├── .gitignore ├── CMakeLists.txt ├── libvectormap ├── CMakeLists.txt ├── include │ ├── Math.h │ └── vector_map.h ├── package.xml └── src │ └── vector_map.cpp ├── projector ├── AreaListDrawObject.cpp ├── AreaListDrawObject.h ├── BitmapBackground.cpp ├── BitmapBackground.h ├── CMakeLists.txt ├── Camera.cpp ├── Camera.h ├── CameraImageBackground.cpp ├── CameraImageBackground.h ├── CameraInfo.yaml ├── DrawObject.h ├── Framebuffer.cpp ├── Framebuffer.h ├── ImageDB.cpp ├── ImageDB.h ├── ImageMatcher.cpp ├── ImageMatcher.h ├── LaneListDrawObject.cpp ├── LaneListDrawObject.h ├── LineListDrawObject.cpp ├── LineListDrawObject.h ├── PointListDrawObject.cpp ├── PointListDrawObject.h ├── Rate.h ├── RectangleDrawObject.cpp ├── RectangleDrawObject.h ├── SceneManager.cpp ├── SceneManager.h ├── ShaderProgram.cpp ├── ShaderProgram.h ├── SignalsDrawObject.cpp ├── SignalsDrawObject.h ├── VectorMapDisplay.cpp ├── VectorMapDisplay.h ├── VectorMapDrawObject.cpp ├── VectorMapDrawObject.h ├── VectorMapMatchWindow.cpp ├── VectorMapMatchWindow.h ├── VectorMapObjects.cpp ├── VectorMapObjects.h ├── VectorMapRenderWidget.cpp ├── VectorMapRenderWidget.h ├── dbReadApp.cpp ├── dbReadApp.h ├── dbreader.cpp ├── debug.h ├── glcapture.png ├── imgtrans.cpp ├── main-tesselation.cpp ├── msgdump.cpp ├── nmea2tf.py ├── package.xml ├── projector_match.cpp ├── projector_node.cpp ├── rectangle.cpp ├── signals2.cpp ├── test4x4 ├── test4x4.cpp ├── testabr.cpp ├── testfragment.glsl ├── testreverse.py ├── testvertex.glsl ├── vector_map_match.cpp ├── velodyne_to_camera.1.yaml ├── velodyne_to_camera.2.yaml ├── velodyne_to_camera.py └── video.cpp ├── renderer ├── VectorMapRenderer.pro ├── VectorMapRenderer.pro.user ├── main.cpp ├── mainwindow.cpp ├── mainwindow.h └── mainwindow.ui ├── signal_pub_node ├── CMakeLists.txt ├── Rate.h ├── msg │ ├── Signal.msg │ └── Signals.msg ├── package.xml └── signals.cpp ├── vectormap_slam ├── AreaList.cpp ├── AreaList.h ├── Axes.png ├── CMakeLists.txt ├── Camera.cpp ├── Camera.h ├── CameraDisplay.cpp ├── CameraDisplay.h ├── Crosswalk.cpp ├── Crosswalk.h ├── DirectionArrow.cpp ├── DirectionArrow.h ├── DrawObjects.cpp ├── DrawObjects.h ├── Formulas.wxm ├── Gutters.cpp ├── Gutters.h ├── ImageDisplay.cpp ├── ImageDisplay.h ├── ImageMatch.cpp ├── ImageMatch.h ├── Jacobian for Projection from Camera Coordinate.wxm ├── LineList.cpp ├── LineList.h ├── LineRenderer.cpp ├── LineRenderer.h ├── LinesRaw.cpp ├── LinesRaw.h ├── MatchWindow.cpp ├── MatchWindow.cpp.bak ├── MatchWindow.h ├── MatchWindow.h.bak ├── Math.h ├── PointSolver.cpp ├── PointSolver.h ├── PointSolver2.cpp ├── PointSolver2.h ├── PolesDraw.cpp ├── PolesDraw.h ├── Pose.cpp ├── Pose.h ├── ProjectionFormula.wxm ├── Rate.h ├── RenderWidget.cpp ├── RenderWidget.h ├── Tesselator.cpp ├── Tesselator.h ├── TrafficLights.cpp ├── TrafficLights.h ├── Transform 2D.wxm ├── debug.h ├── imagefilter.py ├── main.cpp ├── mask.png ├── mask.xcf ├── nmi.py ├── notes │ └── flowchart_for_line_projection.txt ├── package.xml ├── params1.txt ├── plotheatmap.py ├── projection.py ├── projectpoint.py ├── reference1.png ├── reference2.png ├── reference3.png ├── solvertest.cpp ├── tess.h ├── test_cases │ └── test1.png ├── test_hist.cpp ├── test_tesselator.cpp ├── tf_eigen.h ├── vector_map.cpp └── vector_map.h └── vislab_3dv_node ├── 3dv.cpp ├── CMakeLists.txt ├── package.xml └── vislab_3dv_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/indigo/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /libvectormap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(libvectormap) 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) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | find_package (Eigen3 REQUIRED) 12 | 13 | 14 | ## Uncomment this if the package has a setup.py. This macro ensures 15 | ## modules and global scripts declared therein get installed 16 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 17 | # catkin_python_setup() 18 | 19 | ################################################ 20 | ## Declare ROS messages, services and actions ## 21 | ################################################ 22 | 23 | ## To declare and build messages, services or actions from within this 24 | ## package, follow these steps: 25 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 26 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 27 | ## * In the file package.xml: 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 30 | ## pulled in transitively but can be declared for certainty nonetheless: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a run_depend tag for "message_runtime" 33 | ## * In this file (CMakeLists.txt): 34 | ## * add "message_generation" and every package in MSG_DEP_SET to 35 | ## find_package(catkin REQUIRED COMPONENTS ...) 36 | ## * add "message_runtime" and every package in MSG_DEP_SET to 37 | ## catkin_package(CATKIN_DEPENDS ...) 38 | ## * uncomment the add_*_files sections below as needed 39 | ## and list every .msg/.srv/.action file to be processed 40 | ## * uncomment the generate_messages entry below 41 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 42 | 43 | ## Generate messages in the 'msg' folder 44 | # add_message_files( 45 | # FILES 46 | # Message1.msg 47 | # Message2.msg 48 | # ) 49 | 50 | ## Generate services in the 'srv' folder 51 | # add_service_files( 52 | # FILES 53 | # Service1.srv 54 | # Service2.srv 55 | # ) 56 | 57 | ## Generate actions in the 'action' folder 58 | # add_action_files( 59 | # FILES 60 | # Action1.action 61 | # Action2.action 62 | # ) 63 | 64 | ## Generate added messages and services with any dependencies listed here 65 | # generate_messages( 66 | # DEPENDENCIES 67 | # std_msgs # Or other packages containing msgs 68 | # ) 69 | 70 | ################################### 71 | ## catkin specific configuration ## 72 | ################################### 73 | ## The catkin_package macro generates cmake config files for your package 74 | ## Declare things to be passed to dependent projects 75 | ## INCLUDE_DIRS: uncomment this if you package contains header files 76 | ## LIBRARIES: libraries you create in this project that dependent projects also need 77 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 78 | ## DEPENDS: system dependencies of this project that dependent projects also need 79 | catkin_package( 80 | INCLUDE_DIRS include 81 | LIBRARIES vectormap 82 | ) 83 | 84 | include_directories( 85 | include 86 | ${catkin_INCLUDE_DIRS} 87 | ${EIGEN3_INCLUDE_DIR} 88 | ) 89 | 90 | 91 | add_library(vectormap STATIC 92 | src/vector_map.cpp 93 | ) 94 | 95 | -------------------------------------------------------------------------------- /libvectormap/include/Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Math.h 3 | * 4 | * Created on: Apr 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SRC_MATH_H_ 9 | #define SRC_MATH_H_ 10 | 11 | #include 12 | #include 13 | 14 | 15 | typedef Eigen::Vector2f Point2; 16 | typedef Eigen::Vector3f Point3; 17 | typedef Eigen::Vector3f Vector3; 18 | typedef Eigen::Vector4f Point4; 19 | typedef Eigen::Vector4f Vector4; 20 | typedef Eigen::Vector2f Vector2; 21 | typedef Eigen::Quaternionf Quaternion; 22 | typedef Eigen::Matrix4f Matrix4; 23 | typedef Eigen::Matrix2f Matrix2; 24 | typedef Eigen::Matrix3f Matrix3; 25 | 26 | 27 | template static F degreeToRadian (const F degree) 28 | { return degree * M_PI / 180; } 29 | 30 | template static F radianToDegree (const F radian) 31 | { return radian * 180 / M_PI; } 32 | 33 | 34 | inline double distance (const Point2 &p1, const Point2 &p2) 35 | { 36 | Vector2 p3 (p2.x()-p1.x(), p2.y()-p1.y()); 37 | return sqrt (p3.squaredNorm()); 38 | } 39 | 40 | 41 | inline double distance (const Point3 &p1, const Point3 &p2) 42 | { 43 | Vector3 p3 (p2.x()-p1.x(), p2.y()-p1.y(), p2.z()-p1.z()); 44 | return sqrt(p3.squaredNorm()); 45 | } 46 | 47 | 48 | inline double distance (const Point4 &p1, const Point4 &p2) 49 | { 50 | Vector4 p3 (p2.x()-p1.x(), p2.y()-p1.y(), p2.z()-p1.z(), p2.w()-p1.w()); 51 | return sqrt(p3.squaredNorm()); 52 | } 53 | 54 | 55 | inline double distance (const double &x1, const double &y1, const double &x2, const double &y2) 56 | { 57 | Point2 p(x1, y1), q(x2, y2); 58 | return distance (p, q); 59 | } 60 | 61 | 62 | inline Vector2 perpendicular (const Vector2 &v) 63 | { return Vector2 (-v.y(), v.x()); } 64 | 65 | 66 | inline void rotate (Vector2 &v, const float angleInDegree) 67 | { 68 | Matrix2 rot; 69 | double c = cos (degreeToRadian(angleInDegree)), 70 | s = sin (degreeToRadian(angleInDegree)); 71 | rot (0, 0) = c; 72 | rot (0, 1) = -s; 73 | rot (1, 0) = s; 74 | rot (1, 1) = c; 75 | v = rot * v; 76 | } 77 | 78 | 79 | #endif /* SRC_MATH_H_ */ 80 | -------------------------------------------------------------------------------- /libvectormap/include/vector_map.h: -------------------------------------------------------------------------------- 1 | #ifndef __VECTOR_MAP__ 2 | #define __VECTOR_MAP__ 3 | 4 | //#include "ros/ros.h" 5 | //#include "std_msgs/String.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "Math.h" 13 | 14 | 15 | 16 | typedef struct{ 17 | int pid; 18 | double bx; 19 | double ly; 20 | double h; 21 | double b; 22 | double l; 23 | int ref; 24 | int mcode1; 25 | int mcode2; 26 | int mcode3; 27 | }Point; 28 | 29 | typedef struct{ 30 | int lid; 31 | int bpid; 32 | int fpid; 33 | int blid; 34 | int flid; 35 | }Line; 36 | 37 | typedef struct{ 38 | int vid; 39 | int pid; 40 | double hang; 41 | double vang; 42 | }Vector; 43 | 44 | 45 | typedef struct{ 46 | int id; 47 | int vid; 48 | int plid; 49 | int type; 50 | int linkid; 51 | }Signal; 52 | 53 | typedef struct{ 54 | int id; 55 | int lid; 56 | double width; 57 | char color; 58 | int type; 59 | int linkid; 60 | }CLine; 61 | 62 | typedef struct{ 63 | int lid; 64 | }Mark; 65 | 66 | typedef struct{ 67 | int did; 68 | double dist; 69 | int pid; 70 | double dir; 71 | double apara; 72 | double r; 73 | double slope; 74 | double cant; 75 | double lw; 76 | double rw; 77 | }DTLane; 78 | 79 | typedef struct{ 80 | int lnid; 81 | int did; 82 | int blid; 83 | int flid; 84 | int bnid; 85 | int fnid; 86 | int jct; 87 | int blid2; 88 | int blid3; 89 | int blid4; 90 | int flid2; 91 | int flid3; 92 | int flid4; 93 | int clossid; 94 | double span; 95 | int lcnt; 96 | int lno; 97 | }Lane; 98 | 99 | 100 | struct Area { 101 | int aid; 102 | int slid, elid; 103 | }; 104 | 105 | 106 | class VectorMap 107 | { 108 | public: 109 | bool loaded; 110 | std::map points; 111 | std::map lines; 112 | std::map clines; 113 | std::map lanes; 114 | std::map dtlanes; 115 | std::map vectors; 116 | std::map signals; 117 | std::map areas; 118 | 119 | int load_points(char *name); 120 | int load_lines(char *name); 121 | int load_lanes(char *name); 122 | int load_vectors(char *name); 123 | int load_signals(char *name); 124 | int load_clines(char *name); 125 | int load_dtlanes(char *name ); 126 | int load_areas (const char *name); 127 | 128 | 129 | VectorMap () : 130 | loaded(false) {} 131 | 132 | 133 | void loadAll (const std::string &dirname); 134 | 135 | inline Point3 getPoint (const int idx) 136 | { 137 | Point3 p; 138 | Point psrc = points[idx]; 139 | p.x() = psrc.bx; p.y() = psrc.ly, p.z() = psrc.h; 140 | return p; 141 | } 142 | 143 | }; 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /libvectormap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | libvectormap 4 | 0.1.0 5 | Common Library to Read Vector Map 6 | 7 | sujiwo 8 | 9 | TODO 10 | 11 | catkin 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /projector/AreaListDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AreaListDrawObject.cpp 3 | * 4 | * Created on: May 12, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "AreaListDrawObject.h" 9 | #include "SceneManager.h" 10 | #include "debug.h" 11 | 12 | 13 | AreaListDrawObject::AreaListDrawObject (VectorMap &v) : 14 | vmap (v) 15 | { 16 | glGenBuffers (1, &vbo); 17 | tessel = gluNewTess (); 18 | 19 | // register callback functions 20 | gluTessCallback (tessel, GLU_TESS_BEGIN_DATA, (void (CALLBACK *)())AreaListDrawObject::__tessBeginCB); 21 | gluTessCallback (tessel, GLU_TESS_END_DATA, (void (CALLBACK *)())AreaListDrawObject::__tessEndCB); 22 | gluTessCallback (tessel, GLU_TESS_VERTEX_DATA, (void (CALLBACK *)())AreaListDrawObject::__tessVertexCB); 23 | } 24 | 25 | 26 | AreaListDrawObject::~AreaListDrawObject () 27 | { 28 | glDeleteBuffers (1, &vbo); 29 | gluDeleteTess (tessel); 30 | } 31 | 32 | 33 | void AreaListDrawObject::initialize () 34 | { 35 | for(std::map::iterator it=vmap.areas.begin(); it!=vmap.areas.end(); it++) { 36 | Area polygon = it->second; 37 | 38 | gluTessBeginPolygon (tessel, this); 39 | gluTessBeginContour (tessel); 40 | 41 | Point3 shpt = vmap.getPoint(vmap.lines[polygon.slid].bpid); 42 | gluTessVertex (tessel, (GLdouble*)&shpt, this); 43 | 44 | for (int lid=polygon.slid; lidgetShader()->use(); 74 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 75 | glEnableClientState (GL_VERTEX_ARRAY); 76 | glVertexPointer (3, GL_FLOAT, 0, 0); 77 | 78 | doSetColor (); 79 | glDrawArrays (GL_TRIANGLES, 0, numOfPoints); 80 | 81 | glDisableClientState (GL_VERTEX_ARRAY); 82 | glBindBuffer (GL_ARRAY_BUFFER, 0); 83 | } 84 | 85 | 86 | void CALLBACK AreaListDrawObject::__tessBeginCB (GLenum type, void *a) 87 | { 88 | AreaListDrawObject *area = (AreaListDrawObject*)a; 89 | } 90 | 91 | 92 | void CALLBACK AreaListDrawObject::__tessEndCB (void *a) 93 | { 94 | AreaListDrawObject *area = (AreaListDrawObject*)a; 95 | } 96 | 97 | 98 | void CALLBACK AreaListDrawObject::__tessVertexCB (void *vtxdata, void *a) 99 | { 100 | AreaListDrawObject *area = (AreaListDrawObject*)a; 101 | Point3 *vtx = (Point3*)vtxdata; 102 | area->areaPoints.push_back(*vtx); 103 | } 104 | 105 | -------------------------------------------------------------------------------- /projector/AreaListDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * AreaListDrawObject.h 3 | * 4 | * Created on: May 12, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SOURCE_DIRECTORY__PROJECTOR_AREALISTDRAWOBJECT_H_ 9 | #define SOURCE_DIRECTORY__PROJECTOR_AREALISTDRAWOBJECT_H_ 10 | 11 | #include "DrawObject.h" 12 | #include "vector_map.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | #ifndef CALLBACK 23 | #define CALLBACK 24 | #endif 25 | 26 | 27 | class AreaListDrawObject: public DrawObject 28 | { 29 | public: 30 | AreaListDrawObject (VectorMap &v); 31 | virtual ~AreaListDrawObject (); 32 | 33 | void initialize (); 34 | void draw (); 35 | 36 | typedef shared_ptr Ptr; 37 | 38 | // tesselation stuffs 39 | static void CALLBACK __tessBeginCB (GLenum type, void *data); 40 | static void CALLBACK __tessEndCB (void *data); 41 | static void CALLBACK __tessVertexCB (void *vtxdata, void *data); 42 | 43 | protected: 44 | VectorMap &vmap; 45 | GLuint vbo; 46 | int numOfPoints; 47 | vector areaPoints; 48 | GLUtesselator *tessel; 49 | }; 50 | 51 | #endif /* SOURCE_DIRECTORY__PROJECTOR_AREALISTDRAWOBJECT_H_ */ 52 | -------------------------------------------------------------------------------- /projector/BitmapBackground.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BitmapBackground.cpp 3 | * 4 | * Created on: Apr 14, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "BitmapBackground.h" 9 | #include 10 | #include "SceneManager.h" 11 | #include "debug.h" 12 | #include 13 | #include 14 | 15 | 16 | using std::string; 17 | 18 | 19 | 20 | const Point2 bitmapPos[4] = { 21 | Point2 (-0.1, -0.1), 22 | Point2 (0.1, -0.1), 23 | Point2 (0.1, 0.1), 24 | Point2 (-0.1, 0.1) 25 | }; 26 | 27 | 28 | BitmapBackground::BitmapBackground(int w, int h) : 29 | width(w), height(h), 30 | _bitmap (NULL) 31 | { 32 | glGenTextures (1, &texId); 33 | glGenBuffers (1, &pbo); 34 | rectbuf = new Framebuffer (width, height); 35 | } 36 | 37 | 38 | BitmapBackground::~BitmapBackground() 39 | { 40 | glDeleteTextures (1, &texId); 41 | glDeleteBuffers (1, &pbo); 42 | delete (_bitmap); 43 | delete (rectbuf); 44 | } 45 | 46 | 47 | void BitmapBackground::initialize() 48 | { 49 | // Initialize bitmap 50 | _bitmap = new uint8_t [BitmapSize]; 51 | for (int i=0; iuseForDrawing (); 68 | 69 | doDraw (); 70 | 71 | glBufferData (GL_PIXEL_UNPACK_BUFFER, BitmapSize, _bitmap, GL_DYNAMIC_DRAW); 72 | 73 | glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texId, 0); 74 | glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB8, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, 0); 75 | 76 | rectbuf->useForReading (); 77 | scene->getMainFramebuffer()->useForDrawing(); 78 | 79 | glBlitFramebuffer (0, 0, width-1, height-1, 0, 0, width-1, height-1, GL_COLOR_BUFFER_BIT, GL_NEAREST); 80 | 81 | glBindBuffer (GL_PIXEL_UNPACK_BUFFER, prevBufId); 82 | glBindTexture (GL_TEXTURE_2D, prevTexId); 83 | } 84 | 85 | 86 | void BitmapBackground::doDraw () 87 | { 88 | // int randX, randY; 89 | // uint8_t randR, randG, randB; 90 | // 91 | // randX = (int)(drand48() * width); 92 | // randY = (int)(drand48() * height); 93 | // randR = (int)(drand48() * 255); 94 | // randG = (int)(drand48() * 255); 95 | // randB = (int)(drand48() * 255); 96 | // 97 | // setColor (randX, randY, randR, randG, randB); 98 | for (int i=0; i<600; i++) { 99 | setColor (i, i, 0, 0, 0); 100 | } 101 | } 102 | 103 | 104 | void BitmapBackground::setColor (int i, int j, const unsigned char r, 105 | const unsigned char g, 106 | const unsigned char b) 107 | { 108 | int p = j*width + i; 109 | _bitmap [p*sizeof(bmpColor)] = r; 110 | _bitmap [p*sizeof(bmpColor) + 1] = g; 111 | _bitmap [p*sizeof(bmpColor) + 2] = b; 112 | } 113 | 114 | 115 | void BitmapBackground::setColor (int x, int y, const Vector3 &color) 116 | { 117 | 118 | } 119 | -------------------------------------------------------------------------------- /projector/BitmapBackground.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BitmapBackground.h 3 | * 4 | * Created on: Apr 14, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef BITMAPBACKGROUND_H_ 9 | #define BITMAPBACKGROUND_H_ 10 | 11 | #include "DrawObject.h" 12 | #include "ShaderProgram.h" 13 | #include "Framebuffer.h" 14 | 15 | 16 | 17 | const uint8_t bmpColor[] = {0xFF, 0xFF, 0xFF}; 18 | 19 | 20 | class BitmapBackground: public DrawObject 21 | { 22 | public: 23 | BitmapBackground () {} 24 | 25 | BitmapBackground (int w, int h); 26 | virtual ~BitmapBackground(); 27 | 28 | virtual void initialize (); 29 | void draw (); 30 | 31 | typedef shared_ptr Ptr; 32 | 33 | inline uint8_t* getBuffer () { return _bitmap; } 34 | 35 | protected: 36 | ShaderProgram shader; 37 | GLuint pbo; 38 | 39 | GLuint texId; 40 | 41 | Framebuffer *rectbuf; 42 | 43 | int width, height; 44 | 45 | uint8_t *_bitmap; 46 | 47 | virtual void doDraw (); 48 | void setColor (int x, int y, const Vector3 &color); 49 | void setColor (int x, int y, const unsigned char r, 50 | const unsigned char g, 51 | const unsigned char b); 52 | }; 53 | 54 | 55 | #define BitmapSize sizeof(bmpColor)*width*height 56 | 57 | 58 | #endif /* BITMAPBACKGROUND_H_ */ 59 | -------------------------------------------------------------------------------- /projector/Camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Camera.h 3 | * 4 | * Created on: Apr 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SRC_CAMERA_H_ 9 | #define SRC_CAMERA_H_ 10 | 11 | 12 | #include "Math.h" 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | class Camera { 19 | public: 20 | Camera (int windowWidth=0, int windowHeight=0); 21 | virtual ~Camera(); 22 | 23 | void setSize (int w, int h); 24 | 25 | inline int getWidth () { return imageWidth; } 26 | inline int getHeight () { return imageHeight; } 27 | 28 | inline Matrix4 &getViewMatrix() 29 | { return viewMatrix; } 30 | 31 | inline Matrix4 &getProjectionMatrix() 32 | { return projectionMatrix; } 33 | 34 | // These functions change View Matrix 35 | void lookAt (const Point3 &eyepos, const Point3 ¢erOfView, const Eigen::Vector3f &up); 36 | void lookAt (const Quaternion &orientation, const Point3 &location); 37 | void lookAt (const tf::Transform &t); 38 | // --- 39 | 40 | // These functions change Projection Matrix 41 | void perspective (float fovy, float aspectRatio, float zNear, float zFar); 42 | void projectionMatrixFromCameraInfo (float fx, float fy, int imageWidth, int imageHeight, float cx, float cy); 43 | void projectionMatrixFromCameraInfo (sensor_msgs::CameraInfo::ConstPtr info); 44 | // --- 45 | 46 | // Playing with view & projection matrices 47 | inline Point3 project (const Point3 &pt) 48 | { 49 | Point4 p4 (pt.x(), pt.y(), pt.z(), 1); 50 | p4 = projectionMatrix * viewMatrix * p4; 51 | return Point3 (p4.x(), p4.y(), p4.z()) / p4.w(); 52 | } 53 | 54 | bool project (const Point3 &pt, int &u, int &v, int reqImageWidth=0, int reqImageHeight=0); 55 | 56 | // Transform a point to camera coordinate 57 | inline Point3 transform (const Point3 &pt) 58 | { 59 | Point4 p4 (pt.x(), pt.y(), pt.z(), 1); 60 | p4 = viewMatrix * p4; 61 | return Point3 (p4.x(), p4.y(), p4.z()) / p4.w(); 62 | } 63 | 64 | inline static Point3 transform (const Point3 &pt, tf::StampedTransform &tfsource) 65 | { 66 | tf::Vector3 pt3 (pt.x(), pt.y(), pt.z()); 67 | tf::Vector3 pt3s = tfsource * pt3; 68 | return Point3 (pt3s.x(), pt3s.y(), pt3s.z()); 69 | } 70 | 71 | 72 | // directly mess up with matrices 73 | void setViewMatrix (const Matrix4 &vm) 74 | { viewMatrix = vm; } 75 | 76 | void setProjectionMatrix (const Matrix4 &pm); 77 | 78 | 79 | private: 80 | Point3 currentPosition; 81 | Quaternion currentOrientation; 82 | 83 | Matrix4 viewMatrix, 84 | projectionMatrix; 85 | 86 | int imageWidth, imageHeight; 87 | 88 | struct { 89 | float fovy, aspectRatio, zNear, zFar; 90 | bool isPerpective; 91 | } _perspective; 92 | }; 93 | 94 | #endif /* SRC_CAMERA_H_ */ 95 | -------------------------------------------------------------------------------- /projector/CameraImageBackground.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CameraImageBackground.cpp 3 | * 4 | * Created on: May 5, 2015 5 | * Author: jiwo 6 | */ 7 | 8 | #include "CameraImageBackground.h" 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | CameraImageBackground::CameraImageBackground ( 15 | ros::NodeHandle &n, 16 | const string &imt, 17 | int _width, int _height) : 18 | node (n), 19 | imageTopic (imt), 20 | imgtrans (node), 21 | disabled (false) 22 | { 23 | rectbuf = NULL; 24 | width = _width, height = _height; 25 | glGenTextures (1, &texId); 26 | glGenBuffers (1, &pbo); 27 | } 28 | 29 | 30 | CameraImageBackground::~CameraImageBackground() 31 | { 32 | //BitmapBackground::~BitmapBackground (); 33 | } 34 | 35 | 36 | void CameraImageBackground::initialize() 37 | { 38 | camSub = imgtrans.subscribe (imageTopic, 10, &CameraImageBackground::imageCatch, this); 39 | } 40 | 41 | 42 | void CameraImageBackground::imageCatch (const sensor_msgs::Image::ConstPtr &imagemsg) 43 | { 44 | if (rectbuf==NULL) { 45 | // initialize image buffer & framebuffer 46 | _bitmap = new uint8_t[BitmapSize]; 47 | rectbuf = new Framebuffer (width, height); 48 | } 49 | 50 | // raw decoding 51 | CvImagePtr convImage = cv_bridge::toCvCopy(imagemsg, "rgb8"); 52 | 53 | // downscaling 54 | cv::Mat bitmap (height, width, CV_8UC3, _bitmap); 55 | cv::resize (convImage->image, bitmap, bitmap.size(), 0, 0); 56 | 57 | // Row-reverse and flipping (rotating by 180 deg) are required for display in OpenGL 58 | cv::flip (bitmap, bitmap, 0); 59 | cv::flip (bitmap, bitmap, -1); 60 | } 61 | 62 | 63 | void CameraImageBackground::draw() 64 | { 65 | if (disabled==true) 66 | return; 67 | if (rectbuf==NULL) 68 | return; 69 | else BitmapBackground::draw(); 70 | } 71 | 72 | 73 | void CameraImageBackground::doDraw () 74 | { 75 | } 76 | -------------------------------------------------------------------------------- /projector/CameraImageBackground.h: -------------------------------------------------------------------------------- 1 | /* 2 | * CameraImageBackground.h 3 | * 4 | * Created on: May 5, 2015 5 | * Author: jiwo 6 | */ 7 | 8 | #ifndef PROJECTOR_CAMERAIMAGEBACKGROUND_H_ 9 | #define PROJECTOR_CAMERAIMAGEBACKGROUND_H_ 10 | 11 | #include "BitmapBackground.h" 12 | #include "ros/ros.h" 13 | #include "sensor_msgs/Image.h" 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | using std::string; 20 | using cv_bridge::CvImagePtr; 21 | 22 | 23 | class CameraImageBackground: public BitmapBackground 24 | { 25 | public: 26 | CameraImageBackground (ros::NodeHandle &n, const string &topic, int _width, int _height); 27 | virtual ~CameraImageBackground(); 28 | 29 | void initialize (); 30 | 31 | typedef shared_ptr Ptr; 32 | 33 | void imageCatch (const sensor_msgs::Image::ConstPtr &imagemsg); 34 | 35 | void draw (); 36 | void doDraw (); 37 | void disable() { disabled = true; } 38 | 39 | 40 | private: 41 | ros::NodeHandle &node; 42 | const string imageTopic; 43 | image_transport::ImageTransport imgtrans; 44 | image_transport::Subscriber camSub; 45 | //CvImagePtr convImage; 46 | bool disabled; 47 | }; 48 | 49 | #endif /* PROJECTOR_CAMERAIMAGEBACKGROUND_H_ */ 50 | -------------------------------------------------------------------------------- /projector/CameraInfo.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1920 2 | image_height: 1440 3 | camera_name: test 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1107.562172, 0.0, 966.241993, 0.0, 1106.053562, 715.166496, 0.0, 0.0, 1.0] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.015529, 0.035475, -0.002315, 0.002689, 0.0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1138.77063, 0.0, 972.014727, 0.0, 0.0, 1138.272217, 710.39206, 0.0, 0.0, 0.0, 1.0, 0.0] -------------------------------------------------------------------------------- /projector/DrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * DrawObject.h 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef DRAWOBJECT_H_ 9 | #define DRAWOBJECT_H_ 10 | 11 | 12 | #include "Math.h" 13 | //#include "PointListDrawObject.h" 14 | #include "ShaderProgram.h" 15 | #include 16 | 17 | 18 | using boost::shared_ptr; 19 | 20 | #define doSetColor() \ 21 | scene->getShader()->setUniform ("objColor", UNIFORM_VECTOR4, &colorThis); 22 | 23 | class SceneManager; 24 | 25 | 26 | class DrawObject 27 | { 28 | public: 29 | DrawObject () : scene(NULL) {} 30 | virtual ~DrawObject () {} 31 | virtual void initialize () = 0; 32 | 33 | inline void _setScene (SceneManager *s) 34 | { scene = s; } 35 | 36 | inline void setColor (const Vector3 &c) 37 | { colorThis = Vector4 (c[0], c[1], c[2], 1.0); } 38 | 39 | inline void setColor ( 40 | const float &r, 41 | const float &g, 42 | const float &b) 43 | { setColor (Vector3 (r, g, b)); } 44 | 45 | inline void setColor (int r, int g, int b) 46 | { setColor (Vector3 ((1.0f/255)*r, (1.0f/255)*g, (1.0f/255)*b)); } 47 | 48 | virtual void draw () = 0; 49 | 50 | typedef shared_ptr Ptr; 51 | 52 | protected: 53 | SceneManager *scene; 54 | Vector4 colorThis; 55 | }; 56 | 57 | 58 | 59 | #endif /* DRAWOBJECT_H_ */ 60 | -------------------------------------------------------------------------------- /projector/Framebuffer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Framebuffer.cpp 3 | * 4 | * Created on: Apr 22, 2015 5 | * Author: jiwo 6 | */ 7 | 8 | #include "Framebuffer.h" 9 | #include "debug.h" 10 | 11 | 12 | Framebuffer::Framebuffer() 13 | { 14 | fbid = 0; 15 | colorRbid = 0; 16 | depthRbid = 0; 17 | imgWidth = -1; 18 | imgHeight = -1; 19 | } 20 | 21 | 22 | Framebuffer::Framebuffer (int w, int h) : 23 | imgWidth (w), imgHeight (h) 24 | { 25 | glGenFramebuffers (1, &fbid); 26 | glGenRenderbuffers (1, &colorRbid); 27 | glGenRenderbuffers (1, &depthRbid); 28 | 29 | glBindFramebuffer (GL_FRAMEBUFFER, fbid); 30 | 31 | glBindRenderbuffer (GL_RENDERBUFFER, colorRbid); 32 | glRenderbufferStorage (GL_RENDERBUFFER, PixelType, imgWidth, imgHeight); 33 | 34 | glBindRenderbuffer (GL_RENDERBUFFER, depthRbid); 35 | glRenderbufferStorage (GL_RENDERBUFFER, GL_DEPTH_COMPONENT, imgWidth, imgHeight); 36 | 37 | glFramebufferRenderbuffer (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, colorRbid); 38 | glFramebufferRenderbuffer (GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, depthRbid); 39 | 40 | GLenum stat = glCheckFramebufferStatus (GL_FRAMEBUFFER); 41 | if (stat != GL_FRAMEBUFFER_COMPLETE) 42 | debug ("Error in framebuffer"); 43 | 44 | glEnable (GL_DEPTH_TEST); 45 | } 46 | 47 | 48 | Framebuffer::~Framebuffer() 49 | { 50 | glDeleteRenderbuffers (1, &depthRbid); 51 | glDeleteRenderbuffers (1, &colorRbid); 52 | glDeleteFramebuffers (1, &fbid); 53 | } 54 | 55 | 56 | void Framebuffer::use() 57 | { 58 | glBindFramebuffer (GL_FRAMEBUFFER, fbid); 59 | } 60 | 61 | 62 | Framebuffer* Framebuffer::getDefault () 63 | { 64 | Framebuffer *defFb = NULL; 65 | 66 | if (defFb==NULL) { 67 | defFb = new Framebuffer (); 68 | } 69 | 70 | return defFb; 71 | } 72 | 73 | 74 | void Framebuffer::copyTo(void *mem) 75 | { 76 | return glReadPixels (0, 0, imgWidth, imgHeight, PixelType, GL_UNSIGNED_BYTE, mem); 77 | } 78 | -------------------------------------------------------------------------------- /projector/Framebuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Framebuffer.h 3 | * 4 | * Created on: Apr 22, 2015 5 | * Author: jiwo 6 | */ 7 | 8 | #ifndef PROJECTOR_FRAMEBUFFER_H_ 9 | #define PROJECTOR_FRAMEBUFFER_H_ 10 | 11 | 12 | #include "GL/glew.h" 13 | #include "GL/gl.h" 14 | #include 15 | #include 16 | 17 | 18 | #define PixelType GL_RGB 19 | #define PixelSize 3 20 | 21 | 22 | class Framebuffer 23 | { 24 | public: 25 | 26 | Framebuffer (); 27 | Framebuffer (int w, int h); 28 | virtual ~Framebuffer(); 29 | 30 | void use (); 31 | 32 | inline GLuint getID () 33 | { return fbid; } 34 | 35 | static Framebuffer* getDefault (); 36 | 37 | inline void useForDrawing () 38 | { glBindFramebuffer (GL_DRAW_FRAMEBUFFER, fbid); } 39 | 40 | inline void releaseForDrawing () 41 | { glBindFramebuffer (GL_DRAW_FRAMEBUFFER, 0); } 42 | 43 | inline void useForReading () 44 | { glBindFramebuffer (GL_READ_FRAMEBUFFER, fbid); } 45 | 46 | inline void releaseForReading () 47 | { glBindFramebuffer (GL_READ_FRAMEBUFFER, 0); } 48 | 49 | // Size of pixel corresponds to GL_RGB 50 | inline size_t getSize () 51 | { return PixelSize * imgWidth * imgHeight; } 52 | 53 | void copyTo (void *mem); 54 | 55 | int width() const { return imgWidth; } 56 | int height() const { return imgHeight;} 57 | 58 | 59 | private: 60 | // framebuffer ID 61 | GLuint fbid; 62 | // color renderbuffer ID 63 | GLuint colorRbid, 64 | // depth renderbuffer ID 65 | depthRbid; 66 | 67 | int imgWidth, imgHeight; 68 | }; 69 | 70 | #endif /* PROJECTOR_FRAMEBUFFER_H_ */ 71 | -------------------------------------------------------------------------------- /projector/ImageDB.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageDB.cpp 3 | * 4 | * Created on: Jun 24, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "ImageDB.h" 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | using std::vector; 15 | 16 | 17 | ImageDB::ImageDB (const string &path) 18 | { 19 | sqlite3_open (path.c_str(), &bagtable); 20 | sqlite3_prepare (bagtable, "SELECT * FROM bags WHERE id=?", -1, &bagRead, NULL); 21 | } 22 | 23 | 24 | ImageDB::~ImageDB() 25 | { 26 | sqlite3_close (bagtable); 27 | } 28 | 29 | 30 | void ImageDB::get (int id, Point3 &position, Quaternion &orientation, cv::Mat &img) 31 | { 32 | int c; 33 | sqlite3_reset (bagRead); 34 | c = sqlite3_bind_int (bagRead, 1, id); 35 | if ((c=sqlite3_step (bagRead)) == SQLITE_ROW) { 36 | 37 | position.x() = (float)sqlite3_column_double(bagRead, 2); 38 | position.y() = (float)sqlite3_column_double(bagRead, 3); 39 | position.z() = (float)sqlite3_column_double(bagRead, 4); 40 | orientation.x() = (float)sqlite3_column_double(bagRead, 5); 41 | orientation.y() = (float)sqlite3_column_double(bagRead, 6); 42 | orientation.z() = (float)sqlite3_column_double(bagRead, 7); 43 | orientation.w() = (float)sqlite3_column_double(bagRead, 8); 44 | 45 | // image blob 46 | const uint8_t *buf = (uint8_t*)sqlite3_column_blob (bagRead, 9); 47 | int len = sqlite3_column_bytes (bagRead, 9); 48 | vector imgbuf (buf, buf+len); 49 | img = cv::imdecode (imgbuf, -1); 50 | return; 51 | } 52 | else { 53 | const char *er = sqlite3_errstr (c); 54 | throw er; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /projector/ImageDB.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageDB.h 3 | * 4 | * Created on: Jun 24, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SOURCE_DIRECTORY__PROJECTOR_IMAGEDB_H_ 9 | #define SOURCE_DIRECTORY__PROJECTOR_IMAGEDB_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include "Math.h" 15 | 16 | 17 | 18 | using std::string; 19 | 20 | 21 | class ImageDB { 22 | public: 23 | ImageDB (const string &path); 24 | ~ImageDB (); 25 | void get (int id, Point3 &position, Quaternion &orientation, cv::Mat &img); 26 | 27 | protected: 28 | sqlite3 *bagtable; 29 | sqlite3_stmt *bagRead; 30 | 31 | }; 32 | 33 | #endif /* SOURCE_DIRECTORY__PROJECTOR_IMAGEDB_H_ */ 34 | -------------------------------------------------------------------------------- /projector/ImageMatcher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageMatcher.cpp 3 | * 4 | * Created on: Jul 17, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | 10 | ImageMatcher::ImageMatcher() { 11 | // TODO Auto-generated constructor stub 12 | 13 | } 14 | 15 | ImageMatcher::~ImageMatcher() { 16 | // TODO Auto-generated destructor stub 17 | } 18 | 19 | 20 | 21 | double ImageMatcher::getEntrophy (cv::Mat src) 22 | { 23 | src.total(); 24 | } 25 | -------------------------------------------------------------------------------- /projector/ImageMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageMatcher.h 3 | * 4 | * Created on: Jul 17, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef PROJECTOR_IMAGEMATCHER_H_ 9 | #define PROJECTOR_IMAGEMATCHER_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include "debug.h" 15 | 16 | 17 | class ImageMatcher { 18 | public: 19 | ImageMatcher(); 20 | virtual ~ImageMatcher(); 21 | 22 | static double getEntrophy (cv::Mat src); 23 | }; 24 | 25 | #endif /* PROJECTOR_IMAGEMATCHER_H_ */ 26 | -------------------------------------------------------------------------------- /projector/LaneListDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LaneListDrawObject.cpp 3 | * 4 | * Created on: Apr 15, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "LaneListDrawObject.h" 9 | 10 | 11 | void LaneListDrawObject::initialize () 12 | { 13 | for(std::map::iterator it=vmap.lanes.begin();it!=vmap.lanes.end();it++) { 14 | Lane l=it->second; 15 | Point3 p1, p2; 16 | p1.x() = vmap.points[l.bnid].bx; 17 | p1.y() = vmap.points[l.bnid].ly; 18 | p1.z() = vmap.points[l.bnid].h; 19 | 20 | //jct.points.push_back(p); 21 | p2.x() = vmap.points[l.fnid].bx; 22 | p2.y() = vmap.points[l.fnid].ly; 23 | p2.z() = vmap.points[l.fnid].h; 24 | //jct.points.push_back(p); 25 | 26 | Point3 r1, r2, r3, r4; 27 | LineListDrawObject::createBox (p1, p2, r1, r2, r3, r4, scale); 28 | points.push_back(r1); 29 | points.push_back(r2); 30 | points.push_back(r3); 31 | points.push_back(r3); 32 | points.push_back(r4); 33 | points.push_back(r2); 34 | } 35 | 36 | setColor (0, 0, 255); 37 | initBuffer (); 38 | } 39 | -------------------------------------------------------------------------------- /projector/LaneListDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LaneListDrawObject.h 3 | * 4 | * Created on: Apr 15, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef LANELISTDRAWOBJECT_H_ 9 | #define LANELISTDRAWOBJECT_H_ 10 | 11 | #include "LineListDrawObject.h" 12 | 13 | class LaneListDrawObject: public LineListDrawObject 14 | { 15 | public: 16 | LaneListDrawObject (VectorMap &v, double scale_) : 17 | LineListDrawObject (v, scale_) {} 18 | 19 | void initialize (); 20 | 21 | typedef shared_ptr Ptr; 22 | }; 23 | 24 | #endif /* LANELISTDRAWOBJECT_H_ */ 25 | -------------------------------------------------------------------------------- /projector/LineListDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LineListDrawObject.cpp 3 | * 4 | * Created on: Apr 14, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "LineListDrawObject.h" 9 | #include "SceneManager.h" 10 | 11 | 12 | 13 | void LineListDrawObject::createBox ( 14 | const Point2 &source1, 15 | const Point2 &source2, 16 | Point2 &result1, 17 | Point2 &result2, 18 | Point2 &result3, 19 | Point2 &result4, 20 | double scale) 21 | { 22 | Vector2 u = source2 - source1; 23 | u.normalize(); 24 | u = u * scale/2; 25 | Point2 s1 = source1 - u, 26 | s2 = source2 + u; 27 | Vector2 v = perpendicular (u); 28 | v.normalize(); 29 | v = v * scale/2; 30 | result1 = s1 - v; 31 | result2 = s1 + v; 32 | result3 = s2 - v; 33 | result4 = s2 + v; 34 | } 35 | 36 | 37 | Point2 Point3to2 (const Point3 &p3) 38 | { return Point2 (p3.x(), p3.y()); } 39 | 40 | 41 | Point3 Point2to3 (const Point2 &p2, double zval) 42 | { return Point3 (p2.x(), p2.y(), zval); } 43 | 44 | 45 | void LineListDrawObject::createBox ( 46 | const Point3 &source1, 47 | const Point3 &source2, 48 | Point3 &result1, 49 | Point3 &result2, 50 | Point3 &result3, 51 | Point3 &result4, 52 | double scale) 53 | { 54 | Point2 s1 = Point3to2 (source1), 55 | s2 = Point3to2 (source2); 56 | Point2 r1, r2, r3, r4; 57 | 58 | createBox (s1, s2, r1, r2, r3, r4, scale); 59 | result1 = Point2to3 (r1, source1.z()); 60 | result2 = Point2to3 (r2, source1.z()); 61 | result3 = Point2to3 (r3, source2.z()); 62 | result4 = Point2to3 (r4, source2.z()); 63 | } 64 | 65 | 66 | LineListDrawObject::LineListDrawObject (VectorMap &v, double sc) : 67 | vmap (v), 68 | scale (sc) 69 | { 70 | glGenBuffers (1, &vbo); 71 | } 72 | 73 | 74 | LineListDrawObject::~LineListDrawObject() 75 | { 76 | glDeleteBuffers (1, &vbo); 77 | } 78 | 79 | 80 | void LineListDrawObject::initialize () 81 | { 82 | for(std::map::iterator it=vmap.lines.begin(); it!=vmap.lines.end(); it++) { 83 | Line l = it->second; 84 | Point3 p1, p2; 85 | p1.x() = vmap.points[l.bpid].bx; 86 | p1.y() = vmap.points[l.bpid].ly; 87 | p1.z() = vmap.points[l.bpid].h; 88 | p2.x() = vmap.points[l.fpid].bx; 89 | p2.y() = vmap.points[l.fpid].ly; 90 | p2.z() = vmap.points[l.fpid].h; 91 | 92 | Point3 r1, r2, r3, r4; 93 | LineListDrawObject::createBox (p1, p2, r1, r2, r3, r4, scale); 94 | points.push_back(r1); 95 | points.push_back(r2); 96 | points.push_back(r3); 97 | points.push_back(r3); 98 | points.push_back(r4); 99 | points.push_back(r2); 100 | } 101 | 102 | setColor(0, 255, 0); 103 | initBuffer (); 104 | } 105 | 106 | 107 | void LineListDrawObject::initBuffer () 108 | { 109 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 110 | 111 | glBufferData (GL_ARRAY_BUFFER, 112 | sizeof(Point3)*points.size(), 113 | points.data(), 114 | GL_STATIC_DRAW); 115 | 116 | glBindBuffer (GL_ARRAY_BUFFER, 0); 117 | } 118 | 119 | 120 | void LineListDrawObject::draw() 121 | { 122 | scene->getShader()->use(); 123 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 124 | glEnableClientState (GL_VERTEX_ARRAY); 125 | glVertexPointer (3, GL_FLOAT, 0, 0); 126 | 127 | doSetColor (); 128 | glDrawArrays (GL_TRIANGLES, 0, points.size()); 129 | //glFlush (); 130 | 131 | glDisableClientState (GL_VERTEX_ARRAY); 132 | glBindBuffer (GL_ARRAY_BUFFER, 0); 133 | } 134 | -------------------------------------------------------------------------------- /projector/LineListDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LineListDrawObject.h 3 | * 4 | * Created on: Apr 14, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef LINELISTDRAWOBJECT_H_ 9 | #define LINELISTDRAWOBJECT_H_ 10 | 11 | #include "DrawObject.h" 12 | #include "vector_map.h" 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | using std::vector; 19 | 20 | 21 | class LineListDrawObject: public DrawObject 22 | { 23 | public: 24 | LineListDrawObject (vector &src, double scale); 25 | LineListDrawObject (VectorMap &v, double scale); 26 | virtual ~LineListDrawObject(); 27 | 28 | void initialize (); 29 | void draw (); 30 | 31 | typedef shared_ptr Ptr; 32 | 33 | protected: 34 | void initBuffer (); 35 | 36 | static void createBox ( 37 | const Point3 &source1, 38 | const Point3 &source2, 39 | Point3 &result1, 40 | Point3 &result2, 41 | Point3 &result3, 42 | Point3 &result4, 43 | double scale); 44 | 45 | static void createBox ( 46 | const Point2 &source1, 47 | const Point2 &source2, 48 | Point2 &result1, 49 | Point2 &result2, 50 | Point2 &result3, 51 | Point2 &result4, 52 | double scale); 53 | 54 | VectorMap &vmap; 55 | vector points; 56 | GLuint vbo; 57 | double scale; 58 | 59 | }; 60 | 61 | #endif /* LINELISTDRAWOBJECT_H_ */ 62 | -------------------------------------------------------------------------------- /projector/PointListDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapDrawObject.cpp 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "PointListDrawObject.h" 9 | #include "SceneManager.h" 10 | 11 | 12 | PointListDrawObject::PointListDrawObject () 13 | { 14 | glGenBuffers (1, &vbo); 15 | } 16 | 17 | 18 | PointListDrawObject::PointListDrawObject (vector lst) : 19 | pointList (lst), 20 | pointSize (1.5) 21 | { 22 | PointListDrawObject (); 23 | } 24 | 25 | 26 | PointListDrawObject::PointListDrawObject (VectorMap &vmap) : 27 | pointSize (1.5) 28 | { 29 | PointListDrawObject (); 30 | 31 | for (int i=1; i<=vmap.points.size(); i++) { 32 | Point3 p3 = vmap.getPoint(i); 33 | pointList.push_back (p3); 34 | } 35 | } 36 | 37 | 38 | PointListDrawObject::~PointListDrawObject () 39 | { 40 | glDeleteBuffers (1, &vbo); 41 | } 42 | 43 | 44 | void PointListDrawObject::initialize () 45 | { 46 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 47 | 48 | int ptlen = pointList.size(); 49 | 50 | // watch out for mismatch size (3 or 4 ? not sure) 51 | glBufferData (GL_ARRAY_BUFFER, sizeof(Point3)*ptlen, pointList.data(), GL_STATIC_DRAW); 52 | 53 | glBindBuffer (GL_ARRAY_BUFFER, 0); 54 | } 55 | 56 | 57 | void PointListDrawObject::draw() 58 | { 59 | scene->getShader()->use(); 60 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 61 | glEnableClientState (GL_VERTEX_ARRAY); 62 | glVertexPointer (3, GL_FLOAT, 0, 0); 63 | 64 | doSetColor (); 65 | glPointSize (pointSize); 66 | glDrawArrays (GL_POINTS, 0, pointList.size()); 67 | 68 | glDisableClientState (GL_VERTEX_ARRAY); 69 | glBindBuffer (GL_ARRAY_BUFFER, 0); 70 | } 71 | -------------------------------------------------------------------------------- /projector/PointListDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapDrawObject.h 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAPDRAWOBJECT_H_ 9 | #define VECTORMAPDRAWOBJECT_H_ 10 | 11 | 12 | #include "DrawObject.h" 13 | #include "vector_map.h" 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | class PointListDrawObject: public DrawObject 23 | { 24 | public: 25 | PointListDrawObject (); 26 | PointListDrawObject (vector); 27 | PointListDrawObject (VectorMap &v); 28 | virtual ~PointListDrawObject(); 29 | 30 | void initialize (); 31 | void draw (); 32 | 33 | inline void add (const Point3 &p) 34 | { pointList.push_back(p); } 35 | 36 | inline size_t size () { return pointList.size(); } 37 | inline void setSize (float s) { pointSize = s; } 38 | 39 | typedef shared_ptr Ptr; 40 | 41 | private: 42 | vector pointList; 43 | GLuint vbo; 44 | float pointSize; 45 | }; 46 | 47 | #endif /* VECTORMAPDRAWOBJECT_H_ */ 48 | -------------------------------------------------------------------------------- /projector/Rate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Rate.h 3 | * 4 | * Created on: Mar 19, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef RATE_H_ 9 | #define RATE_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace boost::posix_time; 17 | 18 | 19 | class Rate { 20 | public: 21 | inline Rate (int hz) 22 | { 23 | assert (hz >= 1); 24 | float microsec = 1e6 / (float)hz; 25 | sleeptime = microseconds (microsec); 26 | lastUpdate = microsec_clock::local_time(); 27 | } 28 | 29 | inline void sleep() 30 | { 31 | ptime curtime = microsec_clock::local_time(); 32 | time_duration delay = curtime - lastUpdate; 33 | if (delay < sleeptime) { 34 | time_duration realSleepTime = sleeptime - delay; 35 | usleep (realSleepTime.total_microseconds()); 36 | } 37 | lastUpdate = microsec_clock::local_time(); 38 | } 39 | 40 | private: 41 | ptime lastUpdate; 42 | time_duration sleeptime; 43 | }; 44 | 45 | #endif /* RATE_H_ */ 46 | -------------------------------------------------------------------------------- /projector/RectangleDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LaneDrawObject.cpp 3 | * 4 | * Created on: Apr 11, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include "RectangleDrawObject.h" 12 | #include "SceneManager.h" 13 | 14 | 15 | using std::vector; 16 | 17 | 18 | RectangleDrawObject::RectangleDrawObject (const Point3 &p1, const Point3 &p2, double sc) : 19 | point1(p1), point2(p2), 20 | scale (sc) 21 | { 22 | glGenBuffers (1, &vbo); 23 | } 24 | 25 | 26 | RectangleDrawObject::~RectangleDrawObject() 27 | { 28 | glDeleteBuffers (1, &vbo); 29 | } 30 | 31 | 32 | void RectangleDrawObject::createBox ( 33 | const Point2 &source1, 34 | const Point2 &source2, 35 | Point2 &result1, 36 | Point2 &result2, 37 | Point2 &result3, 38 | Point2 &result4, 39 | double scale) 40 | { 41 | double grad1 = ( source2.y() - source2.y() ) / ( source2.x() - source2.x() ), 42 | grad2 = 1 / grad1; 43 | 44 | double s = grad2 / sqrt (1 + grad2), 45 | c = s / grad2; 46 | double len2 = scale / 2; 47 | Point2 v (len2 * c, len2 * s); 48 | 49 | result1 = source1 - v, 50 | result2 = source1 + v, 51 | result3 = source2 - v, 52 | result4 = source2 + v; 53 | } 54 | 55 | 56 | Point2 Point3to2 (const Point3 &p3) 57 | { return Point2 (p3.x(), p3.y()); } 58 | 59 | 60 | Point3 Point2to3 (const Point2 &p2, double zval) 61 | { return Point3 (p2.x(), p2.y(), zval); } 62 | 63 | 64 | void RectangleDrawObject::createBox ( 65 | const Point3 &source1, 66 | const Point3 &source2, 67 | Point3 &result1, 68 | Point3 &result2, 69 | Point3 &result3, 70 | Point3 &result4, 71 | double scale) 72 | { 73 | Point2 s1 = Point3to2 (source1), 74 | s2 = Point3to2 (source2); 75 | Point2 r1, r2, r3, r4; 76 | 77 | createBox (s1, s2, r1, r2, r3, r4, scale); 78 | result1 = Point2to3 (r1, s1.z()); 79 | result2 = Point2to3 (r2, s1.z()); 80 | result3 = Point2to3 (r3, s2.z()); 81 | result4 = Point2to3 (r4, s2.z()); 82 | } 83 | 84 | 85 | void RectangleDrawObject::initialize () 86 | { 87 | Point3 rect[4]; 88 | createBox (point1, point2, rect[0], rect[1], rect[2], rect[3], scale); 89 | 90 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 91 | glBufferData (GL_ARRAY_BUFFER, sizeof(Point3)*4, rect, GL_STATIC_DRAW); 92 | glBindBuffer (GL_ARRAY_BUFFER, 0); 93 | } 94 | 95 | 96 | void RectangleDrawObject::draw() 97 | { 98 | scene->getShader()->use(); 99 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 100 | glEnableClientState (GL_VERTEX_ARRAY); 101 | glVertexPointer (3, GL_FLOAT, 0, 0); 102 | 103 | //setColor(Vector3(1.0, 1.0, 0)); 104 | doSetColor (); 105 | glPointSize (4.0); 106 | glDrawArrays (GL_TRIANGLE_STRIP, 0, 4); 107 | //glFlush (); 108 | 109 | glDisableClientState (GL_VERTEX_ARRAY); 110 | glBindBuffer (GL_ARRAY_BUFFER, 0); 111 | } 112 | -------------------------------------------------------------------------------- /projector/RectangleDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LaneDrawObject.h 3 | * 4 | * Created on: Apr 11, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef RECTANGLEDRAWOBJECT_H_ 9 | #define RECTANGLEDRAWOBJECT_H_ 10 | 11 | #include "DrawObject.h" 12 | #include 13 | 14 | 15 | class RectangleDrawObject: public DrawObject 16 | { 17 | public: 18 | RectangleDrawObject (const Point3 &p1, const Point3 &p2, double sc); 19 | virtual ~RectangleDrawObject(); 20 | 21 | void initialize (); 22 | void draw (); 23 | 24 | typedef shared_ptr Ptr; 25 | 26 | static void createBox ( 27 | const Point2 &source1, 28 | const Point2 &source2, 29 | Point2 &result1, 30 | Point2 &result2, 31 | Point2 &result3, 32 | Point2 &result4, 33 | double scale); 34 | 35 | static void createBox ( 36 | const Point3 &source1, 37 | const Point3 &source2, 38 | Point3 &result1, 39 | Point3 &result2, 40 | Point3 &result3, 41 | Point3 &result4, 42 | double scale); 43 | 44 | private: 45 | Point3 point1, point2; 46 | GLuint vbo; 47 | double scale; 48 | }; 49 | 50 | #endif /* RECTANGLEDRAWOBJECT_H_ */ 51 | -------------------------------------------------------------------------------- /projector/SceneManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SceneManager.h 3 | * 4 | * Created on: Apr 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SRC_SCENEMANAGER_H_ 9 | #define SRC_SCENEMANAGER_H_ 10 | 11 | 12 | #include 13 | #include "Math.h" 14 | #include "ShaderProgram.h" 15 | #include "Camera.h" 16 | #include "DrawObject.h" 17 | #include "Framebuffer.h" 18 | #include "CameraImageBackground.h" 19 | 20 | 21 | using std::vector; 22 | 23 | 24 | class SceneManager 25 | { 26 | public: 27 | 28 | SceneManager (int windowWidth, int windowHeight); 29 | virtual ~SceneManager(); 30 | static SceneManager& getInstance (int windowWidth=-1, int windowHeight=-1); 31 | 32 | ShaderProgram* getShader () { return &shader; } 33 | Camera* getCamera () { return camera; } 34 | Framebuffer *getMainFramebuffer () { return framebuffer; } 35 | 36 | static void display (); 37 | void update (); 38 | void update2 (); 39 | void loopEvent (); 40 | 41 | void addObject (DrawObject::Ptr obj); 42 | 43 | void setPose (const tf::Transform &tnf); 44 | void setPose (const Point3 &position, const Quaternion &orientation); 45 | void setPose (); 46 | 47 | static void keyboardFunc (unsigned char key, int x, int y); 48 | 49 | void setCameraBackground (CameraImageBackground::Ptr cam) 50 | { cameraBackground = cam; } 51 | 52 | void captureToDisk (); 53 | void offsetPos (float dx, float dy, float dz); 54 | void offsetReset (); 55 | void offsetRotation (float roll, float pitch, float yaw); 56 | 57 | private: 58 | ShaderProgram shader; 59 | Camera *camera; 60 | Framebuffer *framebuffer, 61 | *defaultFramebuffer; 62 | 63 | vector drawableList; 64 | 65 | tf::Transform currentPose; 66 | tf::Vector3 positionOffset; 67 | 68 | int windowWidth, windowHeight; 69 | CameraImageBackground::Ptr cameraBackground; 70 | }; 71 | 72 | #endif /* SRC_SCENEMANAGER_H_ */ 73 | -------------------------------------------------------------------------------- /projector/ShaderProgram.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ShaderProgram.cpp 3 | * 4 | * Created on: Jan 26, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include "ShaderProgram.h" 12 | #include 13 | 14 | using std::ifstream; 15 | using std::ios; 16 | using std::cout; 17 | using std::cerr; 18 | using std::endl; 19 | 20 | 21 | ShaderProgram::ShaderProgram(bool s) 22 | { 23 | shader = glCreateProgram (); 24 | } 25 | 26 | 27 | ShaderProgram::~ShaderProgram() 28 | { 29 | // TODO Auto-generated destructor stub 30 | } 31 | 32 | 33 | string readContents (ifstream &inpFile) 34 | { 35 | std::stringstream buffer; 36 | buffer << inpFile.rdbuf(); 37 | 38 | return buffer.str(); 39 | } 40 | 41 | 42 | void ShaderProgram::setShaderProgramString (string inp, GLenum type) 43 | { 44 | GLuint shaderObj = glCreateShader (type); 45 | const char *csrc = inp.c_str(); 46 | glShaderSource (shaderObj, 1, &csrc, NULL); 47 | glCompileShader (shaderObj); 48 | 49 | GLint compiled; 50 | glGetShaderiv (shaderObj, GL_COMPILE_STATUS, &compiled); 51 | if (!compiled) { 52 | GLsizei len; 53 | glGetShaderiv (shaderObj, GL_INFO_LOG_LENGTH, &len); 54 | GLchar* log = new GLchar[len+1]; 55 | glGetShaderInfoLog (shaderObj, len, &len, log); 56 | cerr << "Shader compilation failed: " << log << endl; 57 | delete[] log; 58 | return; 59 | } 60 | 61 | glAttachShader (shader, shaderObj); 62 | } 63 | 64 | 65 | void ShaderProgram::link() 66 | { 67 | glLinkProgram (shader); 68 | GLint linked; 69 | glGetProgramiv (shader, GL_LINK_STATUS, &linked); 70 | if (!linked) { 71 | cerr << "Shader link failed" << endl; 72 | } 73 | //glUseProgram (shader); 74 | } 75 | 76 | 77 | ShaderProgram ShaderProgram::fromFile(string vtxInpFilename, string frgInpFilename) 78 | { 79 | ShaderProgram prg (true); 80 | ifstream vtxfd (vtxInpFilename.c_str()); 81 | ifstream frgfd (frgInpFilename.c_str()); 82 | 83 | string vtxStr = readContents (vtxfd); 84 | string frgStr = readContents (frgfd); 85 | frgfd.close (); 86 | vtxfd.close (); 87 | 88 | prg.setShaderProgramString(vtxStr, GL_VERTEX_SHADER); 89 | prg.setShaderProgramString(frgStr, GL_FRAGMENT_SHADER); 90 | 91 | prg.link(); 92 | 93 | return prg; 94 | } 95 | 96 | 97 | ShaderProgram ShaderProgram::fromString(string vtxInpStr, string frgInpStr) 98 | { 99 | ShaderProgram prg (true); 100 | 101 | prg.setShaderProgramString (vtxInpStr, GL_VERTEX_SHADER); 102 | prg.setShaderProgramString (frgInpStr, GL_FRAGMENT_SHADER); 103 | prg.link(); 104 | 105 | return prg; 106 | } 107 | 108 | 109 | bool ShaderProgram::setUniform (string varName, int type, const void *val, bool transpose) 110 | { 111 | GLint loc = glGetUniformLocation (shader, varName.c_str()); 112 | if (loc==-1) 113 | return false; 114 | 115 | switch (type) { 116 | 117 | case UNIFORM_FLOAT1: { 118 | float v = *((float*)val); 119 | glUniform1f (loc, v); 120 | } break; 121 | 122 | case UNIFORM_VECTOR4: { 123 | float *v = (float*)val; 124 | glUniform4fv (loc, 1, v); 125 | } break; 126 | 127 | case UNIFORM_MAT4F: { 128 | float *v = (float*)val; 129 | glUniformMatrix4fv (loc, 1, transpose, v); 130 | } 131 | } 132 | 133 | return true; 134 | } 135 | -------------------------------------------------------------------------------- /projector/ShaderProgram.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ShaderProgram.h 3 | * 4 | * Created on: Jan 26, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SHADERPROGRAM_H_ 9 | #define SHADERPROGRAM_H_ 10 | 11 | #include "GL/glew.h" 12 | #include "GL/gl.h" 13 | #include 14 | 15 | 16 | using std::string; 17 | 18 | 19 | enum { 20 | UNIFORM_FLOAT1 = 0, 21 | UNIFORM_FLOAT3 = 1, 22 | UNIFORM_VECTOR4 = 2, 23 | UNIFORM_MAT4F = 3, 24 | }; 25 | 26 | 27 | class ShaderProgram 28 | { 29 | public: 30 | ShaderProgram () {} 31 | ShaderProgram (bool s); 32 | virtual ~ShaderProgram(); 33 | 34 | static ShaderProgram 35 | fromFile ( 36 | string vertexShaderInputFile, 37 | string fragShaderInputFile); 38 | 39 | static ShaderProgram 40 | fromString ( 41 | string vertexShaderInputString, 42 | string fragmentShaderInputString); 43 | 44 | bool setUniform (string varName, int type, const void *val, bool transpose=GL_FALSE); 45 | 46 | inline void use () 47 | { 48 | glUseProgram (0); 49 | glUseProgram (shader); 50 | } 51 | 52 | inline void release () { glUseProgram (0); } 53 | 54 | private: 55 | GLuint shader; 56 | void setShaderProgramString (string inp, GLenum shaderType); 57 | void link (); 58 | }; 59 | 60 | #endif /* SHADERPROGRAM_H_ */ 61 | -------------------------------------------------------------------------------- /projector/SignalsDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SignalsDrawObject.h 3 | * 4 | * Created on: Apr 13, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SIGNALSDRAWOBJECT_H_ 9 | #define SIGNALSDRAWOBJECT_H_ 10 | 11 | #include "DrawObject.h" 12 | #include "vector_map.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | const float lampRadius = 0.3; 23 | 24 | 25 | #define SIGNAL_RED 1 26 | #define SIGNAL_GREEN 2 27 | #define SIGNAL_YELLOW 3 28 | 29 | 30 | class Camera; 31 | 32 | 33 | struct SignalCircle { 34 | Point3 center; 35 | vector points; 36 | vector idx; 37 | unsigned int color; 38 | unsigned int id; 39 | 40 | bool project (Camera &c, int &u, int &v, float &radInPixel); 41 | Point3 transform (Camera &c); 42 | }; 43 | 44 | 45 | 46 | class SignalsDrawObject: public DrawObject 47 | { 48 | public: 49 | SignalsDrawObject (VectorMap &v, bool gl=true); 50 | virtual ~SignalsDrawObject(); 51 | 52 | void initialize (); 53 | 54 | void draw (); 55 | 56 | typedef shared_ptr Ptr; 57 | 58 | vector &getSignalList () { return signalObjects; } 59 | 60 | private: 61 | VectorMap &vmap; 62 | vector signalCenterPoints; 63 | 64 | vector allPts; 65 | vector signalObjects; 66 | 67 | GLuint vbo; 68 | static Vector3 angle2UnitVector (const float Hang, const float Vang); 69 | static vector signalPoint2Circle (const Point3 ¢erPoint, float Hang, float Vang); 70 | static vector signalPoint2Circle (VectorMap &map, const Signal &sgn); 71 | 72 | }; 73 | 74 | #endif /* SIGNALSDRAWOBJECT_H_ */ 75 | -------------------------------------------------------------------------------- /projector/VectorMapDisplay.cpp: -------------------------------------------------------------------------------- 1 | #include "VectorMapDisplay.h" 2 | 3 | 4 | VectorMapDisplay::VectorMapDisplay (const string &mapdir, int _width, int _height) : 5 | scene (SceneManager::getInstance(_width, _height)) 6 | { 7 | vectormap.loadAll (mapdir); 8 | signals = SignalsDrawObject::Ptr(new SignalsDrawObject (vectormap)); 9 | 10 | scene.addObject (signals); 11 | } 12 | 13 | 14 | VectorMapDisplay::~VectorMapDisplay () 15 | { 16 | 17 | } 18 | -------------------------------------------------------------------------------- /projector/VectorMapDisplay.h: -------------------------------------------------------------------------------- 1 | #include "SceneManager.h" 2 | #include "PointListDrawObject.h" 3 | #include "SignalsDrawObject.h" 4 | #include "LineListDrawObject.h" 5 | #include "LaneListDrawObject.h" 6 | #include "vector_map.h" 7 | #include 8 | 9 | 10 | using std::string; 11 | 12 | 13 | class VectorMapDisplay 14 | { 15 | public: 16 | VectorMapDisplay (const string &mapdir, int _width, int _height); 17 | ~VectorMapDisplay (); 18 | 19 | private: 20 | VectorMap vectormap; 21 | int width, height; 22 | SceneManager &scene; 23 | SignalsDrawObject::Ptr signals; 24 | }; 25 | -------------------------------------------------------------------------------- /projector/VectorMapDrawObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapDrawObject.cpp 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "VectorMapDrawObject.h" 9 | 10 | VectorMapDrawObject::VectorMapDrawObject(VectorMap *m) : 11 | map (m) 12 | { 13 | glGenBuffers (1, &vbo); 14 | } 15 | 16 | 17 | VectorMapDrawObject::~VectorMapDrawObject() 18 | { 19 | glDeleteBuffers (1, &vbo); 20 | } 21 | 22 | 23 | void VectorMapDrawObject::initialize() 24 | { 25 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 26 | 27 | int ptlen = map->points.size(); 28 | for (int i=0; igetPoint(i); 30 | points.push_back(p); 31 | } 32 | // watch out for mismatch size 33 | glBufferData (GL_ARRAY_BUFFER, sizeof(PointXYZ)*ptlen, points.data(), GL_STATIC_DRAW); 34 | 35 | glBindBuffer (GL_ARRAY_BUFFER, 0); 36 | } 37 | 38 | 39 | void VectorMapDrawObject::draw() 40 | { 41 | shader->use(); 42 | glBindBuffer (GL_ARRAY_BUFFER, vbo); 43 | glEnableClientState (GL_VERTEX_ARRAY); 44 | glVertexPointer (3, GL_FLOAT, 4*sizeof(float), 0); 45 | 46 | setColor(Vector3(1.0, 1.0, 0)); 47 | glDrawArrays (GL_POINTS, 0, points.size()); 48 | 49 | glDisableClientState (GL_VERTEX_ARRAY); 50 | glBindBuffer (GL_ARRAY_BUFFER, 0); 51 | } 52 | -------------------------------------------------------------------------------- /projector/VectorMapDrawObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapDrawObject.h 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAPDRAWOBJECT_H_ 9 | #define VECTORMAPDRAWOBJECT_H_ 10 | 11 | #include "DrawObject.h" 12 | #include "vector_map.h" 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | using std::vector; 19 | 20 | 21 | class VectorMapDrawObject: public DrawObject 22 | { 23 | public: 24 | VectorMapDrawObject (VectorMap *m); 25 | virtual ~VectorMapDrawObject(); 26 | 27 | void initialize (); 28 | void draw (); 29 | 30 | private: 31 | VectorMap *map; 32 | GLuint vbo; 33 | vector points; 34 | }; 35 | 36 | #endif /* VECTORMAPDRAWOBJECT_H_ */ 37 | -------------------------------------------------------------------------------- /projector/VectorMapMatchWindow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapMatchWindow.h 3 | * 4 | * Created on: Jul 6, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SOURCE_DIRECTORY__PROJECTOR_VECTORMAPMATCHWINDOW_H_ 9 | #define SOURCE_DIRECTORY__PROJECTOR_VECTORMAPMATCHWINDOW_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "VectorMapRenderWidget.h" 23 | #include 24 | 25 | 26 | 27 | class VectorMapMatchWindow: public QMainWindow { 28 | Q_OBJECT 29 | 30 | public: 31 | VectorMapMatchWindow(); 32 | virtual ~VectorMapMatchWindow(); 33 | 34 | void setPositionText (const double &x, const double &y, const double &z); 35 | void setImageSource (const cv::Mat &image); 36 | 37 | bool isClosed () 38 | { return closed; } 39 | int getImageWidth () 40 | { return imagebmp->width(); } 41 | int getImageHeight () 42 | { return imagebmp->height(); } 43 | 44 | inline void render () { if (glcanvas!=NULL) glcanvas->update(); } 45 | 46 | void setPose (const geometry_msgs::PoseStamped::ConstPtr pose); 47 | 48 | 49 | protected: 50 | QWidget *centralWidget; 51 | QVBoxLayout *mainLayout; 52 | 53 | QWidget *imagesContainer; 54 | 55 | QLabel *imageSrc; 56 | QImage *imagebmp; 57 | VectorMapRenderWidget *glcanvas; 58 | bool imageSrcIsSet; 59 | 60 | // position & orientation 61 | QLineEdit *wposx, *wposy, *wposz; 62 | QLineEdit *wroll, *wpitch, *wyaw; 63 | 64 | bool closed; 65 | void closeEvent (QCloseEvent *event); 66 | 67 | sensor_msgs::CameraInfo cameraInfo; 68 | }; 69 | 70 | 71 | class LabelWithTextBox: public QWidget 72 | { 73 | Q_OBJECT 74 | public: 75 | LabelWithTextBox (const string &textlabel, QWidget *parent=NULL); 76 | void setText (const string &text) 77 | { textEntry->setText(QString::fromStdString(text)); } 78 | 79 | protected: 80 | QLineEdit *textEntry; 81 | }; 82 | 83 | #endif /* SOURCE_DIRECTORY__PROJECTOR_VECTORMAPMATCHWINDOW_H_ */ 84 | -------------------------------------------------------------------------------- /projector/VectorMapObjects.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapObjects.cpp 3 | * 4 | * Created on: Jul 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "VectorMapObjects.h" 9 | #include "VectorMapRenderWidget.h" 10 | #include "debug.h" 11 | #include 12 | 13 | 14 | VectorMapObject::VectorMapObject (VectorMapRenderWidget *w) : 15 | glwidget(w) 16 | {} 17 | 18 | 19 | void VectorMapObject::initBuffer() 20 | { 21 | vbuffer = new QGLBuffer (QGLBuffer::VertexBuffer); 22 | vbuffer->bind(); 23 | vbuffer->setUsagePattern(QGLBuffer::StaticDraw); 24 | bool t = vbuffer->create(); 25 | if (!t) 26 | debug ("Unable to create vertex buffer"); 27 | vbuffer->release(); 28 | } 29 | 30 | 31 | VectorMapObject::~VectorMapObject() 32 | { 33 | vbuffer->destroy(); 34 | delete (vbuffer); 35 | } 36 | 37 | 38 | void PointList::initialize() 39 | { 40 | initBuffer (); 41 | vbuffer->bind(); 42 | int len = pointsrc.size(); 43 | vbuffer->allocate(pointsrc.data(), sizeof(Point3)*len); 44 | //glBufferData (GL_ARRAY_BUFFER, sizeof(Point3)*len, pointsrc.data(), GL_STATIC_DRAW); 45 | vbuffer->release(); 46 | } 47 | 48 | 49 | void PointList::draw() 50 | { 51 | vbuffer->bind(); 52 | glEnableClientState (GL_VERTEX_ARRAY); 53 | glVertexPointer (3, GL_FLOAT, 0, 0); 54 | 55 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 56 | glPointSize (size); 57 | glDrawArrays (GL_POINTS, 0, pointsrc.size()); 58 | 59 | glDisableClientState (GL_VERTEX_ARRAY); 60 | vbuffer->release(); 61 | } 62 | 63 | 64 | TRectangle::TRectangle ( 65 | const Point3 &p1, 66 | const Point3 &p2, 67 | const Point3 &p3, 68 | const Point3 &p4, 69 | VectorMapRenderWidget *w) : 70 | VectorMapObject (w) 71 | { 72 | points[0] = p1; 73 | points[1] = p2; 74 | points[2] = p3; 75 | points[3] = p4; 76 | color = Vector4 (1.0, 1.0, 1.0, 1.0); 77 | } 78 | 79 | 80 | void TRectangle::initialize() 81 | { 82 | initBuffer (); 83 | vbuffer->bind(); 84 | 85 | const int sz = sizeof(points); 86 | vbuffer->allocate(points, sz); 87 | vbuffer->release(); 88 | } 89 | 90 | 91 | void TRectangle::draw() 92 | { 93 | vbuffer->bind (); 94 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 95 | glEnableClientState (GL_VERTEX_ARRAY); 96 | glVertexPointer (3, GL_FLOAT, 0, 0); 97 | glDrawArrays (GL_TRIANGLE_FAN, 0, 4); 98 | glDisableClientState (GL_VERTEX_ARRAY); 99 | vbuffer->release (); 100 | } 101 | 102 | 103 | 104 | #define QT_NO_KEYWORDS 105 | #undef signals 106 | #include "vector_map.h" 107 | 108 | 109 | MapPoints::MapPoints (const string &path, VectorMapRenderWidget *w) : 110 | PointList(w) 111 | { 112 | map = new VectorMap (); 113 | map->loadAll (path); 114 | 115 | for (int i=1; i<=map->points.size(); i++) { 116 | Point3 p3 = map->getPoint(i); 117 | pointsrc.push_back (p3); 118 | } 119 | } 120 | 121 | 122 | MapPoints::~MapPoints () 123 | { 124 | delete (map); 125 | } 126 | -------------------------------------------------------------------------------- /projector/VectorMapObjects.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapObjects.h 3 | * 4 | * Created on: Jul 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAPOBJECTS_H_ 9 | #define VECTORMAPOBJECTS_H_ 10 | 11 | #include "Math.h" 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | class VectorMapRenderWidget; 18 | using std::string; 19 | 20 | 21 | class VectorMapObject { 22 | public: 23 | VectorMapObject(VectorMapRenderWidget *w); 24 | virtual ~VectorMapObject(); 25 | 26 | virtual void initialize() = 0; 27 | virtual void draw() = 0; 28 | 29 | inline void setColor (const Vector3 &c) 30 | { color = Vector4 (c[0], c[1], c[2], 1.0); } 31 | 32 | inline void setColor ( 33 | const float &r, 34 | const float &g, 35 | const float &b) 36 | { setColor (Vector3 (r, g, b)); } 37 | 38 | inline void setColor (int r, int g, int b) 39 | { setColor (Vector3 ((1.0f/255)*r, (1.0f/255)*g, (1.0f/255)*b)); } 40 | 41 | 42 | protected: 43 | Vector4 color; 44 | QGLBuffer *vbuffer; 45 | VectorMapRenderWidget *glwidget; 46 | void initBuffer (); 47 | }; 48 | 49 | 50 | 51 | #include 52 | using std::vector; 53 | 54 | 55 | class PointList: public VectorMapObject { 56 | public: 57 | PointList (VectorMapRenderWidget *w) : 58 | VectorMapObject(w), size(1.0) {} 59 | 60 | void initialize (); 61 | void draw (); 62 | 63 | inline void add (const Point3 &p) 64 | { pointsrc.push_back(p); } 65 | 66 | inline void setPointSize (float f) {size=f;} 67 | 68 | protected: 69 | vector pointsrc; 70 | float size; 71 | }; 72 | 73 | 74 | class TRectangle: public VectorMapObject { 75 | public: 76 | TRectangle (const Point3 &p1, const Point3 &p2, const Point3 &p3, const Point3 &p4, VectorMapRenderWidget *w); 77 | void initialize (); 78 | void draw (); 79 | 80 | private: 81 | Point3 points[4]; 82 | }; 83 | 84 | 85 | 86 | class VectorMap; 87 | 88 | class MapPoints : public PointList 89 | { 90 | public: 91 | MapPoints (const string &path, VectorMapRenderWidget *w); 92 | ~MapPoints (); 93 | // void initialize (); 94 | // void draw (); 95 | 96 | private: 97 | VectorMap *map; 98 | }; 99 | 100 | 101 | inline QVector4D color2qv (const Vector4 &v) 102 | { 103 | return QVector4D (v.x(), v.y(), v.z(), v.w()); 104 | } 105 | 106 | 107 | #endif /* VECTORMAPOBJECTS_H_ */ 108 | -------------------------------------------------------------------------------- /projector/VectorMapRenderWidget.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapRenderWidget.cpp 3 | * 4 | * Created on: Jul 7, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "VectorMapRenderWidget.h" 9 | #include 10 | #include "Math.h" 11 | #include "debug.h" 12 | 13 | 14 | using std::string; 15 | 16 | 17 | string defaultVertexShaderString ( 18 | "#version 110\n" 19 | "uniform mat4 modelMat,\n" 20 | " viewMat,\n" 21 | " projectionMat;\n" 22 | "\n" 23 | "void main()\n" 24 | "{ gl_Position = projectionMat * viewMat * modelMat * gl_Vertex; }\n" 25 | ); 26 | 27 | string defaultFragmentShaderString ( 28 | "#version 110\n" 29 | "uniform vec4 objColor;" 30 | "\n" 31 | "void main() \n" 32 | "{ gl_FragColor = objColor; }\n" 33 | ); 34 | 35 | 36 | 37 | VectorMapRenderWidget::VectorMapRenderWidget(QWidget *parent) : 38 | QGLViewer(parent), 39 | camera (NULL), 40 | hasInit (false) 41 | {} 42 | 43 | 44 | VectorMapRenderWidget::~VectorMapRenderWidget() 45 | { 46 | // TODO Auto-generated destructor stub 47 | delete (shader); 48 | for (VectorMapObject* obj : drawObjects) { 49 | delete (obj); 50 | } 51 | } 52 | 53 | 54 | void VectorMapRenderWidget::setSize (int width, int height) 55 | { 56 | setFixedSize (width, height); 57 | hasInit = true; 58 | } 59 | 60 | 61 | void VectorMapRenderWidget::initializeGL () 62 | { 63 | if (!camera) 64 | camera = new Camera (this->width(), this->height()); 65 | 66 | camera->lookAt ( 67 | Point3 (0, -1, 1), 68 | Point3 (0, 1, 1), 69 | Vector3 (0, 0, 0) 70 | ); 71 | camera->perspective(45.0, (float)camera->getWidth()/(float)camera->getHeight(), 1.5, 6); 72 | 73 | shader = new QGLShaderProgram(); 74 | shader->addShaderFromSourceCode (QGLShader::Vertex, defaultVertexShaderString.c_str()); 75 | shader->addShaderFromSourceCode (QGLShader::Fragment, defaultFragmentShaderString.c_str()); 76 | shader->link(); 77 | shader->bind(); 78 | 79 | for (VectorMapObject* obj : drawObjects) { 80 | obj->initialize(); 81 | } 82 | } 83 | 84 | 85 | void VectorMapRenderWidget::resizeGL (int w, int h) 86 | { 87 | glViewport (0, 0, (GLsizei)w, (GLsizei)h); 88 | camera->setSize(w, h); 89 | } 90 | 91 | 92 | void VectorMapRenderWidget::paintGL () 93 | { 94 | if (hasInit==false) 95 | return; 96 | 97 | glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 98 | cameraToShader (); 99 | for (VectorMapObject* obj : drawObjects) { 100 | obj->draw(); 101 | } 102 | glFlush (); 103 | } 104 | 105 | 106 | #define eigen2mat4x4(eigm, mat) \ 107 | {\ 108 | float *__m__ = &mat[0][0];\ 109 | memcpy (__m__, eigm.data(), sizeof(float)*16);\ 110 | } 111 | 112 | 113 | #define eigen2mat3x3(eigm, mat) \ 114 | {\ 115 | float *__m__ = &mat[0][0];\ 116 | memcpy (__m__, eigm.data(), sizeof(float)*9);\ 117 | } 118 | 119 | 120 | 121 | void VectorMapRenderWidget::cameraToShader () 122 | { 123 | int mmid = shader->uniformLocation("objColor"); 124 | Matrix4 ident = Matrix4::Identity (); 125 | GLfloat m[4][4]; 126 | eigen2mat4x4 (ident, m); 127 | shader->setUniformValue ("modelMat", m); 128 | eigen2mat4x4 (camera->getViewMatrix(), m); 129 | shader->setUniformValue ("viewMat", m); 130 | eigen2mat4x4 (camera->getProjectionMatrix(), m); 131 | shader->setUniformValue ("projectionMat", m); 132 | return; 133 | } 134 | 135 | 136 | void VectorMapRenderWidget::update() 137 | { if (hasInit==true) return paintGL(); } 138 | 139 | 140 | void VectorMapRenderWidget::setPose (double position[3], double orientation[4]) 141 | { 142 | // We directly create view matrix here 143 | // Vector3 camPos (position[0], position[1], position[2]); 144 | // Matrix3 camRot; 145 | // Matrix4 viewMat = Matrix4::Identity(); 146 | // Quaternion camRotq (orientation[0], orientation[1], orientation[2], orientation[3]); 147 | // camRot = camRotq.toRotationMatrix(); 148 | // viewMat.block<3, 3>(0, 0) = camRot.transpose(); 149 | // viewMat.block<3, 1>(0, 3) = -camRot.transpose() * camPos; 150 | 151 | // Matrix4 rotfix = Matrix4::Identity(); 152 | // rotfix (1,1) = -1; 153 | // rotfix (2,2) = -1; 154 | // rotfix (3,3) = 1; 155 | // viewMat = rotfix * viewMat; 156 | 157 | // camera->setViewMatrix (viewMat); 158 | Point3 curPos (position[0], position[1], position[2]); 159 | Point3 target = curPos + Vector3 (0, 1, 0); 160 | camera->lookAt ( 161 | curPos, 162 | target, 163 | Vector3 (0, 0, 1) 164 | ); 165 | Matrix4 &viewmat = camera->getViewMatrix(); 166 | std::cout << viewmat << std::endl; 167 | } 168 | 169 | 170 | TGLWidget::TGLWidget(QWidget *parent) : 171 | QGLWidget (QGLFormat(QGL::SampleBuffers), parent) 172 | {} 173 | 174 | 175 | TGLWidget::~TGLWidget() 176 | {} 177 | 178 | 179 | void TGLWidget::initializeGL () 180 | {} 181 | 182 | 183 | void TGLWidget::paintGL () 184 | {} 185 | 186 | 187 | void TGLWidget::resizeGL () 188 | {} 189 | 190 | -------------------------------------------------------------------------------- /projector/VectorMapRenderWidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapRenderWidget.h 3 | * 4 | * Created on: Jul 7, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAPRENDERWIDGET_H_ 9 | #define VECTORMAPRENDERWIDGET_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include "Camera.h" 16 | #include "VectorMapObjects.h" 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | class VectorMapRenderWidget: public QGLViewer { 23 | Q_OBJECT 24 | public: 25 | VectorMapRenderWidget(QWidget *parent); 26 | virtual ~VectorMapRenderWidget(); 27 | 28 | void setSize (int width, int height); 29 | 30 | Camera* getCamera() { return camera; } 31 | QGLShaderProgram* getShader() { return shader; } 32 | void update (); 33 | 34 | inline void forceInit (int w, int h) 35 | { 36 | initializeGL (); 37 | setSize (w, h); 38 | } 39 | 40 | inline void addObject (VectorMapObject* d) 41 | { drawObjects.push_back(d); } 42 | 43 | void setPose (double position[3], double orientation[4]); 44 | 45 | 46 | protected: 47 | void initializeGL (); 48 | void paintGL (); 49 | void resizeGL (int w, int h); 50 | 51 | private: 52 | Camera *camera; 53 | QGLShaderProgram *shader; 54 | 55 | vector drawObjects; 56 | void cameraToShader (); 57 | 58 | bool hasInit; 59 | }; 60 | 61 | 62 | class TGLWidget : public QGLWidget { 63 | Q_OBJECT 64 | public: 65 | TGLWidget (QWidget *parent); 66 | ~TGLWidget (); 67 | 68 | protected: 69 | void initializeGL (); 70 | void paintGL (); 71 | void resizeGL (); 72 | }; 73 | 74 | 75 | #endif /* VECTORMAPRENDERWIDGET_H_ */ 76 | -------------------------------------------------------------------------------- /projector/dbReadApp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * dbReadApp.cpp 3 | * 4 | * Created on: Jun 24, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "dbReadApp.h" 9 | #include 10 | #include 11 | 12 | 13 | dbReadApp::dbReadApp (ImageDB *dbsrc, int argc, char **argv) : 14 | QApplication (argc, argv), 15 | data (dbsrc) 16 | { 17 | window = new QMainWindow (); 18 | centralWidget = new QWidget (window); 19 | 20 | mainLayout = new QVBoxLayout (centralWidget); 21 | centralWidget->setLayout (mainLayout); 22 | window->setCentralWidget (centralWidget); 23 | 24 | image1 = new QLabel (centralWidget); 25 | updateImage (1); 26 | mainLayout->addWidget (image1); 27 | 28 | window->show(); 29 | } 30 | 31 | 32 | dbReadApp::~dbReadApp() 33 | { 34 | // TODO Auto-generated destructor stub 35 | } 36 | 37 | 38 | void dbReadApp::updateImage (int id) 39 | { 40 | 41 | } 42 | 43 | 44 | QPixmap dbReadApp::imageDataToPixmap (int id, Point3 &position, Quaternion &orientation) 45 | { 46 | cv::Mat image; 47 | data->get (id, position, orientation, image); 48 | QImage *imgbuffer = new QImage (image.data, image.cols, image.rows, QImage::Format_RGB888); 49 | return QPixmap::fromImage (*imgbuffer); 50 | } 51 | -------------------------------------------------------------------------------- /projector/dbReadApp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * dbReadApp.h 3 | * 4 | * Created on: Jun 24, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SOURCE_DIRECTORY__PROJECTOR_DBREADAPP_H_ 9 | #define SOURCE_DIRECTORY__PROJECTOR_DBREADAPP_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "Math.h" 20 | #include "ImageDB.h" 21 | 22 | 23 | 24 | #define ImageWidth 640 25 | #define ImageHeight 480 26 | 27 | 28 | 29 | class dbReadApp : public QApplication 30 | { 31 | public: 32 | dbReadApp (ImageDB *dbsrc, int argc, char **argv); 33 | virtual ~dbReadApp(); 34 | 35 | 36 | protected: 37 | 38 | // window elements 39 | ImageDB *data; 40 | QMainWindow *window; 41 | QWidget *centralWidget; 42 | QVBoxLayout *mainLayout; 43 | 44 | // display elements 45 | QLabel *image1; 46 | 47 | QPixmap imageDataToPixmap (int id, Point3 &position, Quaternion &orientation); 48 | void updateImage (int id); 49 | }; 50 | 51 | #endif /* SOURCE_DIRECTORY__PROJECTOR_DBREADAPP_H_ */ 52 | -------------------------------------------------------------------------------- /projector/dbreader.cpp: -------------------------------------------------------------------------------- 1 | #include "VectorMapDisplay.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "ImageDB.h" 8 | #include "dbReadApp.h" 9 | 10 | 11 | 12 | int main (int argc, char *argv[]) 13 | { 14 | ImageDB rosbag (argv[1]); 15 | Point3 position; 16 | Quaternion orientation; 17 | cv::Mat image; 18 | 19 | dbReadApp reader (&rosbag, argc, argv); 20 | reader.exec (); 21 | 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /projector/debug.h: -------------------------------------------------------------------------------- 1 | /* 2 | * debug.h 3 | * 4 | * Created on: Dec 10, 2014 5 | * Author: jiwo 6 | */ 7 | 8 | #ifndef DEBUG_H_ 9 | #define DEBUG_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | 19 | inline void debug (const char *strfmt, ...) 20 | { 21 | va_list args; 22 | va_start (args, strfmt); 23 | vprintf (strfmt, args); 24 | va_end (args); 25 | fprintf (stdout, "\n"); 26 | } 27 | 28 | 29 | inline void _getGlError () 30 | { 31 | GLenum ercode = glGetError (); 32 | if (ercode != GL_NO_ERROR) { 33 | const GLubyte *errstr = gluErrorString (ercode); 34 | debug ("GL Error (%d): %s", ercode, errstr); 35 | } 36 | } 37 | 38 | 39 | #define glwrap(cmd) \ 40 | cmd; \ 41 | _getGlError (); 42 | 43 | 44 | 45 | #endif /* DEBUG_H_ */ 46 | -------------------------------------------------------------------------------- /projector/glcapture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/projector/glcapture.png -------------------------------------------------------------------------------- /projector/imgtrans.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/Header.h" 3 | #include "sensor_msgs/Image.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | using std::string; 14 | using cv_bridge::CvImagePtr; 15 | 16 | int defWidth = 1024, 17 | defHeight = 768; 18 | 19 | image_transport::Publisher camPub; 20 | 21 | 22 | void imageCatch (const sensor_msgs::Image::ConstPtr &imagemsg) 23 | { 24 | static int frameId = 0; 25 | 26 | CvImagePtr convImage = cv_bridge::toCvCopy(imagemsg, "rgb8"); 27 | cv::Mat bitmap (defHeight, defWidth, CV_8UC3); 28 | cv::resize (convImage->image, bitmap, bitmap.size(), 0, 0); 29 | 30 | //cv::flip (bitmap, bitmap, 0); 31 | //flipping 32 | cv::flip (bitmap, bitmap, -1); 33 | 34 | // change brightness 35 | bitmap = bitmap + cv::Scalar(50, 50, 50); 36 | 37 | std_msgs::Header header; 38 | header.seq = frameId++; 39 | header.frame_id = "CameraX"; 40 | header.stamp = ros::Time::now(); 41 | cv_bridge::CvImage bmpexport (header, "rgb8", bitmap); 42 | camPub.publish (bmpexport.toImageMsg()); 43 | } 44 | 45 | 46 | int main (int argc, char *argv[]) 47 | { 48 | // ROS business 49 | ros::init(argc, argv, "map_points", ros::init_options::AnonymousName); 50 | ros::NodeHandle rosnode; 51 | 52 | image_transport::ImageTransport imgtrans (rosnode); 53 | image_transport::Subscriber camSub = imgtrans.subscribe(argv[1], 10, imageCatch); 54 | camPub = imgtrans.advertise ("processImage", 1); 55 | 56 | ros::spin(); 57 | } 58 | -------------------------------------------------------------------------------- /projector/msgdump.cpp: -------------------------------------------------------------------------------- 1 | #include "Math.h" 2 | #include "Rate.h" 3 | #include 4 | #include 5 | #include 6 | #include "sensor_msgs/Image.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | 19 | tf::StampedTransform transform; 20 | image_transport::ImageTransport *imgtrans; 21 | image_transport::Subscriber camSub; 22 | //cv_bridge::CvImageConstPtr imageBkg; 23 | sensor_msgs::Image::ConstPtr imageBkg; 24 | bool isNewImage = false; 25 | sqlite3 *bagtable = NULL; 26 | sqlite3_stmt *bagIns = NULL; 27 | 28 | 29 | 30 | void getTransform () 31 | { 32 | static tf::TransformListener listener; 33 | 34 | // target_frame source_frame 35 | listener.waitForTransform ("camera", "japan_7", ros::Time(), ros::Duration(10.0)); 36 | listener.lookupTransform ("camera", "japan_7", ros::Time(), transform); 37 | } 38 | 39 | 40 | void cleanUp () 41 | { 42 | sqlite3_finalize(bagIns); 43 | sqlite3_close(bagtable); 44 | } 45 | 46 | 47 | void interrupt (int s) 48 | { 49 | cleanUp (); 50 | exit(1); 51 | } 52 | 53 | 54 | void echoMsg () 55 | { 56 | int w, h; 57 | tf::Vector3 &p = transform.getOrigin(); 58 | tf::Quaternion o = transform.getRotation(); 59 | 60 | if (isNewImage) { 61 | w = imageBkg->width, h = imageBkg->height; 62 | isNewImage = false; 63 | 64 | cv_bridge::CvImagePtr imagecopy = cv_bridge::toCvCopy(imageBkg, "rgb8"); 65 | // encode image 66 | bool r; 67 | std::vector imgbuf; 68 | r = cv::imencode(".jpg", imagecopy->image, imgbuf); 69 | 70 | sqlite3_reset (bagIns); 71 | sqlite3_bind_double(bagIns, 1, imageBkg->header.stamp.toSec()); 72 | sqlite3_bind_double(bagIns, 2, p.x()); 73 | sqlite3_bind_double(bagIns, 3, p.y()); 74 | sqlite3_bind_double(bagIns, 4, p.z()); 75 | sqlite3_bind_double(bagIns, 5, o.x()); 76 | sqlite3_bind_double(bagIns, 6, o.y()); 77 | sqlite3_bind_double(bagIns, 7, o.z()); 78 | sqlite3_bind_double(bagIns, 8, o.w()); 79 | sqlite3_bind_blob (bagIns, 9, imgbuf.data(), imgbuf.size(), SQLITE_STATIC); 80 | int s = sqlite3_step (bagIns); 81 | if (s != SQLITE_DONE) { 82 | const char *sqErrMsg = sqlite3_errstr(s); 83 | printf ("Unable to execute sqlite: %s\n", sqErrMsg); 84 | } 85 | } 86 | else { 87 | w = h = 0; 88 | } 89 | 90 | // printf ("%f %f %f, %f %f %f %f, (%d %d)\n", 91 | // p.x(), p.y(), p.z(), 92 | // o.w(), o.x(), o.y(), o.z(), 93 | // w, h); 94 | } 95 | 96 | 97 | void imageCatch (const sensor_msgs::Image::ConstPtr &imagemsg) 98 | { 99 | imageBkg = imagemsg; 100 | isNewImage = true; 101 | } 102 | 103 | 104 | int main (int argc, char *argv[]) 105 | { 106 | // ROS business 107 | ros::init(argc, argv, "grab", ros::init_options::AnonymousName); 108 | ros::NodeHandle rosnode; 109 | 110 | imgtrans = new image_transport::ImageTransport (rosnode); 111 | camSub = imgtrans->subscribe ("/camera/image_raw", 10, imageCatch); 112 | 113 | signal (SIGINT, interrupt); 114 | Rate loop (25); 115 | 116 | // table 117 | sqlite3_open (argv[1], &bagtable); 118 | sqlite3_prepare (bagtable, 119 | "INSERT INTO bags(tstamp,px,py,pz,rx,ry,rz,rw,image) VALUES(?,?,?,?,?,?,?,?,?)", 120 | -1, &bagIns, NULL); 121 | 122 | while (true) { 123 | 124 | ros::spinOnce(); 125 | 126 | try { 127 | getTransform (); 128 | } catch (tf::TransformException &exc) { 129 | } 130 | 131 | echoMsg (); 132 | 133 | loop.sleep(); 134 | } 135 | 136 | return 0; 137 | 138 | } 139 | -------------------------------------------------------------------------------- /projector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | projector 4 | 0.0.0 5 | The projector package 6 | 7 | 8 | 9 | 10 | sujiwo 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /projector/projector_match.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_points.cpp 3 | * 4 | * Created on: Apr 11, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | 9 | #include "SceneManager.h" 10 | #include "PointListDrawObject.h" 11 | #include "SignalsDrawObject.h" 12 | #include "LineListDrawObject.h" 13 | #include "LaneListDrawObject.h" 14 | #include "CameraImageBackground.h" 15 | #include "AreaListDrawObject.h" 16 | #include "Rate.h" 17 | #include 18 | #include "vector_map.h" 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | 25 | VectorMap vectormap; 26 | Eigen::Vector3f position; 27 | Eigen::Quaternionf orientation; 28 | tf::StampedTransform transform; 29 | 30 | 31 | #define WINDOW_WIDTH 1024 32 | #define WINDOW_HEIGHT 768 33 | #define LINE_SCALE 0.05 34 | 35 | 36 | 37 | void getTransform (Eigen::Quaternionf &ori, Point3 &pos) 38 | { 39 | static tf::TransformListener listener; 40 | 41 | // target_frame source_frame 42 | // listener.waitForTransform ("japan_7", "camera", ros::Time(), ros::Duration(10.0)); 43 | // listener.lookupTransform ("japan_7", "camera", ros::Time(), transform); 44 | listener.waitForTransform ("camera", "japan_7", ros::Time(), ros::Duration(10.0)); 45 | listener.lookupTransform ("camera", "japan_7", ros::Time(), transform); 46 | 47 | // printf ("PosX:%f, PosY:%f, PosZ:%f\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); 48 | // tf::Vector3 &p = transform.getOrigin(); 49 | // tf::Quaternion o = transform.getRotation(); 50 | // pos.x()=p.x(); pos.y()=p.y(); pos.z()=p.z(); 51 | // ori.w()=o.w(); ori.x()=o.x(); ori.y()=o.y(); ori.z()=o.z(); 52 | } 53 | 54 | 55 | void debugMatrix (const Matrix4 &prjm) 56 | { 57 | std::cout << "###" << std::endl; 58 | for (int r=0; r<4; r++) { 59 | for (int c=0; c<4; c++) { 60 | std::cout << prjm(r, c) << " "; 61 | } 62 | std::cout << std::endl; 63 | } 64 | std::cout << "###" << std::endl; 65 | } 66 | 67 | 68 | void cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr camInfoMsg) 69 | { 70 | SceneManager &scene = SceneManager::getInstance(); 71 | scene.getCamera()->projectionMatrixFromCameraInfo (camInfoMsg); 72 | //debugMatrix (scene.getCamera()->getProjectionMatrix()); 73 | } 74 | 75 | 76 | void interrupt (int s) 77 | { 78 | exit(1); 79 | } 80 | 81 | 82 | int main (int argc, char **argv) 83 | { 84 | SceneManager &app = SceneManager::getInstance(WINDOW_WIDTH, WINDOW_HEIGHT); 85 | vectormap.loadAll(argv[1]); 86 | 87 | // ROS business 88 | ros::init(argc, argv, "map_points", ros::init_options::AnonymousName); 89 | ros::NodeHandle rosnode; 90 | ros::Subscriber cameraInfoSubscriber = rosnode.subscribe ("/camera/camera_info", 100, cameraInfoCallback); 91 | 92 | PointListDrawObject::Ptr pointList (new PointListDrawObject (vectormap)); 93 | SignalsDrawObject::Ptr signals (new SignalsDrawObject (vectormap)); 94 | LineListDrawObject::Ptr lineList (new LineListDrawObject (vectormap, LINE_SCALE)); 95 | //LaneListDrawObject::Ptr laneList (new LaneListDrawObject (vectormap, LINE_SCALE)); 96 | //AreaListDrawObject::Ptr areaList (new AreaListDrawObject (vectormap)); 97 | CameraImageBackground::Ptr cameraImage (new CameraImageBackground (rosnode, "/camera/image_raw", WINDOW_WIDTH, WINDOW_HEIGHT)); 98 | //cameraImage->disable(); 99 | 100 | // Drawing order depend on this list 101 | app.addObject (cameraImage); 102 | app.setCameraBackground (cameraImage); 103 | 104 | app.addObject (pointList); 105 | app.addObject (signals); 106 | app.addObject (lineList); 107 | //app.addObject (laneList); 108 | //app.addObject (areaList); 109 | 110 | pointList->setColor(255, 255, 0); 111 | //videoImg->setColor(0, 255, 255); 112 | 113 | signal (SIGINT, interrupt); 114 | 115 | Rate loop (25); 116 | while (true) { 117 | 118 | ros::spinOnce(); 119 | 120 | try { 121 | getTransform (orientation, position); 122 | } catch (tf::TransformException &exc) { 123 | } 124 | 125 | app.setPose (transform); 126 | app.update(); 127 | app.loopEvent(); 128 | loop.sleep(); 129 | } 130 | 131 | return 0; 132 | } 133 | -------------------------------------------------------------------------------- /projector/projector_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_points.cpp 3 | * 4 | * Created on: Apr 11, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | 9 | #include "SceneManager.h" 10 | #include "PointListDrawObject.h" 11 | #include "SignalsDrawObject.h" 12 | #include "LineListDrawObject.h" 13 | #include "LaneListDrawObject.h" 14 | #include "CameraImageBackground.h" 15 | #include "AreaListDrawObject.h" 16 | #include "Rate.h" 17 | #include 18 | #include "vector_map.h" 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | 25 | VectorMap vectormap; 26 | Eigen::Vector3f position; 27 | Eigen::Quaternionf orientation; 28 | tf::StampedTransform transform; 29 | 30 | 31 | #define WINDOW_WIDTH 1024 32 | #define WINDOW_HEIGHT 768 33 | #define LINE_SCALE 0.05 34 | 35 | 36 | 37 | void getTransform (Eigen::Quaternionf &ori, Point3 &pos) 38 | { 39 | static tf::TransformListener listener; 40 | 41 | // target_frame source_frame 42 | // listener.waitForTransform ("japan_7", "camera", ros::Time(), ros::Duration(10.0)); 43 | // listener.lookupTransform ("japan_7", "camera", ros::Time(), transform); 44 | listener.waitForTransform ("camera1", "japan_7", ros::Time(), ros::Duration(10.0)); 45 | listener.lookupTransform ("camera1", "japan_7", ros::Time(), transform); 46 | 47 | printf ("PosX:%f, PosY:%f, PosZ:%f\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); 48 | // tf::Vector3 &p = transform.getOrigin(); 49 | // tf::Quaternion o = transform.getRotation(); 50 | // pos.x()=p.x(); pos.y()=p.y(); pos.z()=p.z(); 51 | // ori.w()=o.w(); ori.x()=o.x(); ori.y()=o.y(); ori.z()=o.z(); 52 | } 53 | 54 | 55 | void debugMatrix (const Matrix4 &prjm) 56 | { 57 | std::cout << "###" << std::endl; 58 | for (int r=0; r<4; r++) { 59 | for (int c=0; c<4; c++) { 60 | std::cout << prjm(r, c) << " "; 61 | } 62 | std::cout << std::endl; 63 | } 64 | std::cout << "###" << std::endl; 65 | } 66 | 67 | 68 | void cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr camInfoMsg) 69 | { 70 | SceneManager &scene = SceneManager::getInstance(); 71 | scene.getCamera()->projectionMatrixFromCameraInfo (camInfoMsg); 72 | //debugMatrix (scene.getCamera()->getProjectionMatrix()); 73 | } 74 | 75 | 76 | void interrupt (int s) 77 | { 78 | exit(1); 79 | } 80 | 81 | 82 | int main (int argc, char **argv) 83 | { 84 | SceneManager &app = SceneManager::getInstance(WINDOW_WIDTH, WINDOW_HEIGHT); 85 | vectormap.loadAll(argv[1]); 86 | 87 | // ROS business 88 | ros::init(argc, argv, "map_points", ros::init_options::AnonymousName); 89 | ros::NodeHandle rosnode; 90 | ros::Subscriber cameraInfoSubscriber = rosnode.subscribe ("/camera1/camera_info", 100, cameraInfoCallback); 91 | 92 | PointListDrawObject::Ptr pointList (new PointListDrawObject (vectormap)); 93 | SignalsDrawObject::Ptr signals (new SignalsDrawObject (vectormap)); 94 | LineListDrawObject::Ptr lineList (new LineListDrawObject (vectormap, LINE_SCALE)); 95 | LaneListDrawObject::Ptr laneList (new LaneListDrawObject (vectormap, LINE_SCALE)); 96 | CameraImageBackground::Ptr videoImg (new CameraImageBackground (rosnode, "/camera1/image_raw", WINDOW_WIDTH, WINDOW_HEIGHT)); 97 | AreaListDrawObject::Ptr areaList (new AreaListDrawObject (vectormap)); 98 | 99 | // Drawing order depend on this list 100 | app.addObject (videoImg); 101 | app.addObject (pointList); 102 | app.addObject (signals); 103 | app.addObject (lineList); 104 | app.addObject (laneList); 105 | app.addObject (areaList); 106 | 107 | pointList->setColor(255, 255, 0); 108 | //videoImg->setColor(0, 255, 255); 109 | 110 | signal (SIGINT, interrupt); 111 | 112 | Rate loop (25); 113 | while (true) { 114 | 115 | ros::spinOnce(); 116 | 117 | try { 118 | getTransform (orientation, position); 119 | } catch (tf::TransformException &exc) { 120 | } 121 | 122 | //app.getCamera()->lookAt(orientation, position); 123 | //app.getCamera()->lookAt (transform); 124 | app.setPose (transform); 125 | 126 | app.update(); 127 | loop.sleep(); 128 | } 129 | 130 | return 0; 131 | } 132 | -------------------------------------------------------------------------------- /projector/rectangle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rectangle.cpp 3 | * 4 | * Created on: Apr 10, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "SceneManager.h" 9 | #include "PointListDrawObject.h" 10 | #include "RectangleDrawObject.h" 11 | #include "BitmapBackground.h" 12 | #include "Rate.h" 13 | #include 14 | 15 | 16 | #define WIDTH 800 17 | #define HEIGHT 600 18 | 19 | 20 | void interrupt (int s) 21 | { 22 | exit(1); 23 | } 24 | 25 | 26 | 27 | int main (int argc, char **argv) 28 | { 29 | Rate loop (25); 30 | SceneManager &scene = SceneManager::getInstance(WIDTH, HEIGHT); 31 | signal (SIGINT, interrupt); 32 | 33 | PointListDrawObject::Ptr pList (new PointListDrawObject); 34 | pList->add (Point3 (0, 0, -2)); 35 | pList->add (Point3 (1, 0, -2)); 36 | pList->add (Point3 (1, 1, -2)); 37 | pList->add (Point3 (0, 1, -2)); 38 | pList->setColor(255, 255, 0); 39 | pList->setSize(6.0); 40 | 41 | //BitmapBackground::Ptr rect (new BitmapBackground(WIDTH, HEIGHT)); 42 | 43 | //scene.addObject (rect); 44 | scene.addObject (pList); 45 | 46 | scene.getCamera()->lookAt ( 47 | Point3(-0.5, -0.5, 2), 48 | Point3(0.5, 0.5, -2), 49 | Vector3(0, 1, 0)); 50 | // scene.getCamera()->lookAt ( 51 | // Point3(0.5, 0.5, 2), 52 | // Point3(0.5, 0.5, -2), 53 | // Vector3(0, 1, 0)); 54 | 55 | scene.getCamera()->perspective (45.0, 56 | (float)WIDTH / (float)HEIGHT, 57 | 1.0, 100); 58 | std::cout << "===projection===\n"; 59 | std::cout << scene.getCamera()->getProjectionMatrix() << std::endl; 60 | std::cout << "===view===\n"; 61 | std::cout << scene.getCamera()->getViewMatrix() << std::endl; 62 | 63 | while (true) { 64 | scene.update(); 65 | loop.sleep(); 66 | } 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /projector/signals2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * signals2.cpp 3 | * 4 | * Created on: Apr 14, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | 9 | 10 | 11 | #include 12 | #include 13 | #include "Camera.h" 14 | #include "Rate.h" 15 | #include "vector_map.h" 16 | #include 17 | #include 18 | #include "SignalsDrawObject.h" 19 | #include 20 | #include 21 | #include "debug.h" 22 | 23 | 24 | 25 | VectorMap vmap; 26 | Camera camera; 27 | 28 | Eigen::Vector3f position; 29 | Eigen::Quaternionf orientation; 30 | 31 | 32 | 33 | void getTransform (Eigen::Quaternionf &ori, Point3 &pos) 34 | { 35 | static tf::TransformListener listener; 36 | tf::StampedTransform trf; 37 | 38 | // target_frame source_frame 39 | listener.waitForTransform ("japan_7", "camera", ros::Time(), ros::Duration(10.0)); 40 | listener.lookupTransform ("japan_7", "camera", ros::Time(), trf); 41 | 42 | tf::Vector3 &p = trf.getOrigin(); 43 | tf::Quaternion o = trf.getRotation(); 44 | pos.x()=p.x(); pos.y()=p.y(); pos.z()=p.z(); 45 | ori.w()=o.w(); ori.x()=o.x(); ori.y()=o.y(); ori.z()=o.z(); 46 | } 47 | 48 | 49 | void debugMatrix (const Matrix4 &prjm) 50 | { 51 | std::cout << "###" << std::endl; 52 | for (int r=0; r<4; r++) { 53 | for (int c=0; c<4; c++) { 54 | std::cout << prjm(r, c) << " "; 55 | } 56 | std::cout << std::endl; 57 | } 58 | std::cout << "###" << std::endl; 59 | } 60 | 61 | 62 | void cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr camInfoMsg) 63 | { 64 | camera.setSize (camInfoMsg->width, camInfoMsg->height); 65 | camera.projectionMatrixFromCameraInfo (camInfoMsg); 66 | } 67 | 68 | 69 | void interrupt (int s) 70 | { 71 | exit(1); 72 | } 73 | 74 | 75 | void echoSignal (SignalsDrawObject &signObj) 76 | { 77 | std::vector &signalList = signObj.getSignalList(); 78 | int countSignal = 0; 79 | 80 | for (int i=0; i 10 | 11 | 12 | int main (int argc, char **argv) 13 | { 14 | float mat[2][2], *pmat; 15 | 16 | mat[0][0] = 1; 17 | mat[1][0] = 2; 18 | mat[0][1] = 3; 19 | mat[1][1] = 4; 20 | 21 | pmat = &mat[0][0]; 22 | for (int i=0; i<4; i++) { 23 | std::cout << pmat[i] << std::endl; 24 | } 25 | 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /projector/testabr.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * testabr.cpp 3 | * 4 | * Created on: Apr 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | class Hewan 15 | { 16 | public: 17 | Hewan () {} 18 | virtual ~Hewan () {} 19 | virtual void suara () {} 20 | }; 21 | 22 | 23 | class Sapi : public Hewan 24 | { 25 | public: 26 | virtual ~Sapi () {} 27 | void suara () 28 | { 29 | std::cout << "Sapi" << std::endl; 30 | } 31 | }; 32 | 33 | 34 | class Kambing : public Hewan 35 | { 36 | public: 37 | virtual ~Kambing () {} 38 | void suara () 39 | { 40 | std::cout << "Kambing" << std::endl; 41 | } 42 | }; 43 | 44 | 45 | int main (int argc, char **argv) 46 | { 47 | std::vector lst; 48 | Sapi h1; 49 | Kambing k1; 50 | 51 | lst.push_back(&h1); 52 | lst.push_back(&k1); 53 | 54 | Hewan *h = lst[1]; 55 | h->suara (); 56 | 57 | exit (0); 58 | } 59 | -------------------------------------------------------------------------------- /projector/testfragment.glsl: -------------------------------------------------------------------------------- 1 | #version 110 2 | 3 | 4 | uniform vec4 objColor; 5 | 6 | 7 | void main () 8 | { 9 | gl_FragColor = objColor; 10 | } 11 | -------------------------------------------------------------------------------- /projector/testreverse.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | lst = [0, 1, 2, 3, 4] 4 | listlen = len(lst) 5 | 6 | for i in range(listlen/2) : 7 | # ps = lst[i] 8 | # pt = lst[listlen-1-i] 9 | # print (pt) 10 | tmp = lst[i] 11 | lst [i] = lst[listlen-1-i] 12 | lst[listlen-1-i] = tmp 13 | print (lst) 14 | -------------------------------------------------------------------------------- /projector/testvertex.glsl: -------------------------------------------------------------------------------- 1 | #version 110 2 | 3 | 4 | uniform mat4 modelMat, 5 | viewMat, 6 | projectionMat; 7 | 8 | 9 | void main () 10 | { 11 | gl_Position = projectionMat * viewMat * modelMat * gl_Vertex; 12 | //gl_Position = gl_Position / gl_Position.w; 13 | } 14 | -------------------------------------------------------------------------------- /projector/vector_map_match.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "debug.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "Rate.h" 8 | #include "VectorMapMatchWindow.h" 9 | #include 10 | #include "sensor_msgs/Image.h" 11 | #include 12 | #include 13 | 14 | 15 | 16 | VectorMapMatchWindow *mainwindow; 17 | 18 | 19 | void gnss_data_recv (const geometry_msgs::PoseStamped::ConstPtr pose) 20 | { 21 | // double 22 | // posx = pose->pose.position.x, 23 | // posy = pose->pose.position.y, 24 | // posz = pose->pose.position.z; 25 | // mainwindow->setPositionText(posx, posy, posz); 26 | mainwindow->setPose (pose); 27 | } 28 | 29 | 30 | void image_data_recv (const sensor_msgs::Image::ConstPtr &imagemsg) 31 | { 32 | cv_bridge::CvImagePtr convImage = cv_bridge::toCvCopy(imagemsg, "rgb8"); 33 | //cv::cvtColor(convImage->image, convImage->image, CV_BGR2RGB); 34 | mainwindow->setImageSource (convImage->image); 35 | } 36 | 37 | 38 | 39 | int main (int argc, char **argv) 40 | { 41 | if (argc != 3) { 42 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 43 | exit (1); 44 | } 45 | 46 | QApplication app (argc, argv); 47 | 48 | ros::init(argc, argv, "vmmatch", ros::init_options::AnonymousName); 49 | ros::NodeHandle rosnode; 50 | ros::Subscriber gnssRecv = rosnode.subscribe ("gnss_pose", 100, gnss_data_recv); 51 | //image_transport::ImageTransport imageTrans (rosnode); 52 | debug ("GL Started"); 53 | // Baris berikut ini membuat masalah dengan glsldb 54 | //image_transport::Subscriber imageSubs = imageTrans.subscribe ("/image_raw", 10, image_data_recv); 55 | ros::Subscriber imgRecv = rosnode.subscribe ("/image_raw", 10, image_data_recv); 56 | debug ("Image Subscribed"); 57 | 58 | mainwindow = new VectorMapMatchWindow (); 59 | mainwindow->show(); 60 | 61 | Rate rate (100); 62 | while (true) { 63 | if (mainwindow->isClosed() == true) 64 | break; 65 | ros::spinOnce(); 66 | app.processEvents(); 67 | mainwindow->render(); 68 | rate.sleep(); 69 | } 70 | 71 | delete (mainwindow); 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /projector/velodyne_to_camera.1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | CameraExtrinsicMat: !!opencv-matrix 3 | rows: 4 4 | cols: 4 5 | dt: d 6 | data: [ -6.7413127756494884e-02, -7.9059743153815254e-02, 7 | 9.9458786802300114e-01, 9.7366713415020467e-01, 8 | 9.9470938445194246e-01, -8.2773983845225496e-02, 9 | 6.0841664027438458e-02, 1.0705537120217135e-01, 10 | 7.7515873789333334e-02, 9.9342741285452885e-01, 11 | 8.4221521595332693e-02, -4.8138384495180309e-01, 0., 0., 0., 1. ] 12 | CameraMat: !!opencv-matrix 13 | rows: 3 14 | cols: 3 15 | dt: d 16 | data: [ 1.0874663358470427e+03, 0., 9.6569192332251282e+02, 0., 17 | 1.0639636885131740e+03, 7.3424300909318515e+02, 0., 0., 1. ] 18 | DistCoeff: !!opencv-matrix 19 | rows: 1 20 | cols: 5 21 | dt: d 22 | data: [ -1.2797147840673967e-01, -2.5605078133871095e-01, 23 | -8.9502318328221631e-03, -1.2977296912611356e-02, 24 | 8.3457258279968671e-01 ] 25 | ImageSize: [ 1920, 1440 ] 26 | ReprojectionError: 6.6540162730970231e-01 27 | -------------------------------------------------------------------------------- /projector/velodyne_to_camera.2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | CameraExtrinsicMat: !!opencv-matrix 3 | rows: 4 4 | cols: 4 5 | dt: d 6 | data: [ -1.0729603276874666e-02, -6.6458208099877458e-02, 7 | 9.9773151808974858e-01, -3.0256588960598274e-01, 8 | -9.9921230711271081e-01, -3.7410377795620509e-02, 9 | -1.3237407135953061e-02, 1.0076000360207968e-01, 10 | 3.8205247388479247e-02, -9.9708764419650819e-01, 11 | -6.6004460929870223e-02, -1.0261617257058873e+00, 0., 0., 0., 1. ] 12 | CameraMat: !!opencv-matrix 13 | rows: 3 14 | cols: 3 15 | dt: d 16 | data: [ 1.4403289066153782e+03, 0., 4.0482616915734189e+02, 0., 17 | 1.4578221749417055e+03, 3.2972067909264871e+02, 0., 0., 1. ] 18 | DistCoeff: !!opencv-matrix 19 | rows: 1 20 | cols: 5 21 | dt: d 22 | data: [ -6.9456305895167741e-02, 1.9827956766186932e+00, 23 | 7.2009387579324282e-03, -2.4592642909512628e-04, 24 | 0 ] 25 | ImageSize: [ 61101, 1 ] 26 | ReprojectionError: 1.9457511126538479e-01 27 | -------------------------------------------------------------------------------- /projector/velodyne_to_camera.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import numpy as np 3 | import cv 4 | import tf.transformations as tfm 5 | import rospy 6 | import tf 7 | import sys 8 | 9 | 10 | def loadSource (filename): 11 | content = cv.Load(filename) 12 | return np.asarray(content) 13 | 14 | 15 | 16 | if __name__ == '__main__' : 17 | # Prepare transformation 18 | filename = sys.argv[1] 19 | ymsrc = loadSource(filename) 20 | print ("Loaded "+filename) 21 | quaternion = tfm.quaternion_from_matrix(ymsrc) 22 | translation = (ymsrc[0,3], ymsrc[1,3], ymsrc[2,3]) 23 | 24 | rospy.init_node ("velodyne_to_camera_publisher") 25 | sender = tf.TransformBroadcaster (100) 26 | rate = rospy.Rate (25) 27 | 28 | while not rospy.is_shutdown() : 29 | sender.sendTransform(translation, quaternion, rospy.get_rostime(), "camera", "velodyne") 30 | print ("Sent") 31 | rate.sleep() 32 | -------------------------------------------------------------------------------- /projector/video.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rectangle.cpp 3 | * 4 | * Created on: Apr 10, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "SceneManager.h" 9 | #include "CameraImageBackground.h" 10 | #include "Rate.h" 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | #define WIDTH 1024 17 | #define HEIGHT 768 18 | 19 | 20 | void interrupt (int s) 21 | { 22 | exit(1); 23 | } 24 | 25 | 26 | void catchMe (const sensor_msgs::Image::ConstPtr &imagemsg) 27 | { 28 | printf ("Hello World Imaging\n"); 29 | } 30 | 31 | 32 | int main (int argc, char **argv) 33 | { 34 | Rate loop (25); 35 | SceneManager &scene = SceneManager::getInstance(WIDTH, HEIGHT); 36 | signal (SIGINT, interrupt); 37 | 38 | // ROS Business 39 | ros::init (argc, argv, "video_view", ros::init_options::AnonymousName); 40 | ros::NodeHandle rosnode; 41 | 42 | CameraImageBackground::Ptr videoRect (new CameraImageBackground (rosnode, "/camera/image_raw", WIDTH, HEIGHT)); 43 | scene.addObject (videoRect); 44 | 45 | scene.getCamera()->lookAt ( 46 | Point3(0.5, -1, 0.5), 47 | Point3(0.5, 1, 0.5), 48 | Vector3(0, 0, 1)); 49 | scene.getCamera()->perspective (45.0, 50 | (float)WIDTH / (float)HEIGHT, 51 | 1.0, 100); 52 | 53 | while (true) { 54 | ros::spinOnce(); 55 | scene.update(); 56 | loop.sleep(); 57 | } 58 | 59 | return 0; 60 | } 61 | -------------------------------------------------------------------------------- /renderer/VectorMapRenderer.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2015-07-06T11:28:41 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = VectorMapRenderer 12 | TEMPLATE = app 13 | 14 | 15 | SOURCES += main.cpp\ 16 | mainwindow.cpp \ 17 | ../libvectormap/src/vector_map.cpp 18 | 19 | HEADERS += mainwindow.h \ 20 | ../libvectormap/include/Math.h \ 21 | ../libvectormap/include/vector_map.h 22 | 23 | FORMS += mainwindow.ui 24 | -------------------------------------------------------------------------------- /renderer/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include 3 | 4 | int main(int argc, char *argv[]) 5 | { 6 | QApplication a(argc, argv); 7 | MainWindow w; 8 | w.show(); 9 | 10 | return a.exec(); 11 | } 12 | -------------------------------------------------------------------------------- /renderer/mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | 4 | MainWindow::MainWindow(QWidget *parent) : 5 | QMainWindow(parent), 6 | ui(new Ui::MainWindow) 7 | { 8 | ui->setupUi(this); 9 | } 10 | 11 | MainWindow::~MainWindow() 12 | { 13 | delete ui; 14 | } 15 | -------------------------------------------------------------------------------- /renderer/mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | 4 | #include 5 | 6 | namespace Ui { 7 | class MainWindow; 8 | } 9 | 10 | class MainWindow : public QMainWindow 11 | { 12 | Q_OBJECT 13 | 14 | public: 15 | explicit MainWindow(QWidget *parent = 0); 16 | ~MainWindow(); 17 | 18 | private: 19 | Ui::MainWindow *ui; 20 | }; 21 | 22 | #endif // MAINWINDOW_H 23 | -------------------------------------------------------------------------------- /renderer/mainwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | MainWindow 3 | 4 | 5 | 6 | 0 7 | 0 8 | 400 9 | 300 10 | 11 | 12 | 13 | MainWindow 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /signal_pub_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(signal_pub_node) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | libvectormap 6 | roscpp 7 | tf 8 | std_msgs 9 | message_generation 10 | ) 11 | 12 | find_package (Eigen3 REQUIRED) 13 | 14 | 15 | add_message_files( 16 | FILES 17 | Signal.msg 18 | Signals.msg 19 | ) 20 | 21 | 22 | generate_messages ( 23 | DEPENDENCIES 24 | std_msgs 25 | ) 26 | 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES signal_pub_node 31 | CATKIN_DEPENDS libvectormap message_runtime 32 | # DEPENDS system_lib 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | ## Specify additional locations of header files 40 | ## Your package locations should be listed before other locations 41 | # include_directories(include) 42 | include_directories( 43 | ${catkin_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | ${libvectormap_INCLUDE_DIRS} 46 | ) 47 | 48 | ## Declare a cpp executable 49 | add_executable( 50 | signal_pub_node 51 | signals.cpp 52 | ) 53 | 54 | ## Add cmake target dependencies of the executable/library 55 | ## as an example, message headers may need to be generated before nodes 56 | # add_dependencies(signal_pub_node_node signal_pub_node_generate_messages_cpp) 57 | 58 | ## Specify libraries to link a library or executable target against 59 | target_link_libraries(signal_pub_node 60 | ${catkin_LIBRARIES} 61 | ${libvectormap_LIBRARIES} 62 | ) 63 | 64 | 65 | 66 | 67 | ############# 68 | ## Install ## 69 | ############# 70 | 71 | # all install targets should use catkin DESTINATION variables 72 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 73 | 74 | ## Mark executable scripts (Python etc.) for installation 75 | ## in contrast to setup.py, you can choose the destination 76 | # install(PROGRAMS 77 | # scripts/my_python_script 78 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 79 | # ) 80 | 81 | ## Mark executables and/or libraries for installation 82 | # install(TARGETS signal_pub_node signal_pub_node_node 83 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 84 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 85 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 86 | # ) 87 | 88 | ## Mark cpp header files for installation 89 | # install(DIRECTORY include/${PROJECT_NAME}/ 90 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 91 | # FILES_MATCHING PATTERN "*.h" 92 | # PATTERN ".svn" EXCLUDE 93 | # ) 94 | 95 | ## Mark other files for installation (e.g. launch and bag files, etc.) 96 | # install(FILES 97 | # # myfile1 98 | # # myfile2 99 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | # ) 101 | 102 | ############# 103 | ## Testing ## 104 | ############# 105 | 106 | ## Add gtest based cpp test target and link libraries 107 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_signal_pub_node.cpp) 108 | # if(TARGET ${PROJECT_NAME}-test) 109 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 110 | # endif() 111 | 112 | ## Add folders to be run by python nosetests 113 | # catkin_add_nosetests(test) 114 | -------------------------------------------------------------------------------- /signal_pub_node/Rate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Rate.h 3 | * 4 | * Created on: Mar 19, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef RATE_H_ 9 | #define RATE_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace boost::posix_time; 17 | 18 | 19 | class Rate { 20 | public: 21 | inline Rate (int hz) 22 | { 23 | assert (hz >= 1); 24 | float microsec = 1e6 / (float)hz; 25 | sleeptime = microseconds (microsec); 26 | lastUpdate = microsec_clock::local_time(); 27 | } 28 | 29 | inline void sleep() 30 | { 31 | ptime curtime = microsec_clock::local_time(); 32 | time_duration delay = curtime - lastUpdate; 33 | if (delay < sleeptime) { 34 | time_duration realSleepTime = sleeptime - delay; 35 | usleep (realSleepTime.total_microseconds()); 36 | } 37 | lastUpdate = microsec_clock::local_time(); 38 | } 39 | 40 | private: 41 | ptime lastUpdate; 42 | time_duration sleeptime; 43 | }; 44 | 45 | #endif /* RATE_H_ */ 46 | -------------------------------------------------------------------------------- /signal_pub_node/msg/Signal.msg: -------------------------------------------------------------------------------- 1 | int32 signalId 2 | int32 u 3 | int32 v 4 | int32 radius 5 | float64 x 6 | float64 y 7 | float64 z 8 | float64 hang 9 | int8 type 10 | int32 linkId 11 | -------------------------------------------------------------------------------- /signal_pub_node/msg/Signals.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Signal[] Signals 3 | -------------------------------------------------------------------------------- /signal_pub_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | signal_pub_node 4 | 0.1.0 5 | Publisher of Signal Lamps from Vector Map 6 | sujiwo 7 | TODO 8 | catkin 9 | libvectormap 10 | message_generation 11 | libvectormap 12 | message_runtime 13 | 14 | -------------------------------------------------------------------------------- /vectormap_slam/AreaList.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AreaList.cpp 3 | * 4 | * Created on: Aug 5, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "AreaList.h" 9 | #include "RenderWidget.h" 10 | #include "vector_map.h" 11 | 12 | 13 | 14 | 15 | 16 | AreaList::AreaList(VectorMap *v, RenderWidget *w) : 17 | VectorMapObject (w), 18 | map (v) 19 | { 20 | this->loadArea (); 21 | setColor (128, 80, 0); 22 | } 23 | 24 | 25 | //void AreaList::loadArea (int areaId, vector &pointsInArea) 26 | //{ 27 | // Area a = map->areas[areaId]; 28 | // pointsInArea = areaToPoints (a, *map); 29 | //} 30 | 31 | 32 | void AreaList::loadArea () 33 | { 34 | int pid = 0; 35 | for(std::map::iterator it=map->areas.begin(); it!=map->areas.end(); it++) { 36 | Area &a = it->second; 37 | Polygon pl = tesselator.tesselize (a, *map); 38 | pointBuffer.insert (pointBuffer.end(), pl.pointlist.begin(), pl.pointlist.end()); 39 | firstPointIndices.push_back(pid); 40 | polygonPointCounters.push_back(pl.pointlist.size()); 41 | polygonTypes.push_back(pl.drawType); 42 | pid += pl.pointlist.size(); 43 | } 44 | } 45 | 46 | 47 | void AreaList::initialize() 48 | { 49 | initBuffer (); 50 | vbuffer->bind(); 51 | int len = pointBuffer.size(); 52 | vbuffer->allocate(pointBuffer.data(), sizeof(Point3)*len); 53 | vbuffer->release(); 54 | } 55 | 56 | 57 | void AreaList::draw() 58 | { 59 | vbuffer->bind(); 60 | glEnableClientState (GL_VERTEX_ARRAY); 61 | glVertexPointer (3, GL_FLOAT, 0, 0); 62 | 63 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 64 | 65 | for (int p=0; prelease(); 71 | } 72 | -------------------------------------------------------------------------------- /vectormap_slam/AreaList.h: -------------------------------------------------------------------------------- 1 | /* 2 | * AreaList.h 3 | * 4 | * Created on: Aug 5, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _AREALIST_H_ 9 | #define _AREALIST_H_ 10 | 11 | #include "DrawObjects.h" 12 | #include "vector_map.h" 13 | #include "Tesselator.h" 14 | 15 | 16 | 17 | 18 | 19 | class AreaList: public VectorMapObject { 20 | public: 21 | AreaList(VectorMap *v, RenderWidget *w); 22 | 23 | void initialize (); 24 | void draw (); 25 | 26 | protected: 27 | VectorMap *map; 28 | vector pointBuffer; 29 | vector firstPointIndices; 30 | vector polygonPointCounters; 31 | vector polygonTypes; 32 | virtual void loadArea (); 33 | Tesselator tesselator; 34 | }; 35 | 36 | #endif /* _AREALIST_H_ */ 37 | -------------------------------------------------------------------------------- /vectormap_slam/Axes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/Axes.png -------------------------------------------------------------------------------- /vectormap_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vectormap_slam) 3 | 4 | INCLUDE (FindPkgConfig) 5 | 6 | # Activate C++11 Features 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | tf 13 | image_transport 14 | cv_bridge 15 | camera_calibration_parsers 16 | rospy 17 | ) 18 | find_package (OpenGL REQUIRED) 19 | find_package (GLUT REQUIRED) 20 | find_package (GLEW REQUIRED) 21 | find_package (Eigen3 REQUIRED) 22 | find_package (Boost REQUIRED) 23 | find_package (OpenCV REQUIRED) 24 | find_package (Qt4 REQUIRED COMPONENTS 25 | QtCore 26 | QtGui 27 | QtOpenGL 28 | QtXml) 29 | 30 | pkg_check_modules (GSL REQUIRED gsl) 31 | 32 | set (CMAKE_BUILD_TYPE Debug) 33 | set_property (GLOBAL 34 | PROPERTY COMPILE_DEFINITIONS DEBUG=1 35 | ) 36 | # For Qt 4, to do moc 37 | include (${QT_USE_FILE}) 38 | set (CMAKE_AUTOMOC ON) 39 | set (CMAKE_INCLUDE_CURRENT_DIR ON) 40 | 41 | 42 | 43 | 44 | catkin_package( 45 | # INCLUDE_DIRS include 46 | # LIBRARIES vectormap_slam 47 | # CATKIN_DEPENDS other_catkin_pkg 48 | # DEPENDS system_lib 49 | ) 50 | 51 | include_directories ( 52 | include 53 | ${EIGEN3_INCLUDE_DIR} 54 | ${OPENGL_INCLUDE_DIR} 55 | ${GLUT_INCLUDE_DIR} 56 | ${SQLITE3_INCLUDE_DIRS} 57 | ${PCL_INCLUDE_DIRS} 58 | ${BOOST_INCLUDE_DIRS} 59 | ${PNG_INCLUDE_DIRS} 60 | ${roscpp_INCLUDE_DIRS} 61 | ${libvectormap_INCLUDE_DIRS} 62 | ${Qt4_INCLUDE_DIRS} 63 | ${GSL_INCLUDE_DIRS} 64 | ) 65 | 66 | 67 | 68 | add_executable ( 69 | vmslam 70 | main.cpp 71 | MatchWindow.cpp 72 | RenderWidget.cpp 73 | ImageDisplay.cpp 74 | DrawObjects.cpp 75 | Camera.cpp 76 | vector_map.cpp 77 | LineList.cpp 78 | LinesRaw.cpp 79 | TrafficLights.cpp 80 | PolesDraw.cpp 81 | ImageMatch.cpp 82 | DirectionArrow.cpp 83 | Pose.cpp 84 | # Crosswalk.cpp 85 | AreaList.cpp 86 | Tesselator.cpp 87 | # Gutters.cpp 88 | PointSolver.cpp 89 | ) 90 | 91 | target_link_libraries( 92 | vmslam 93 | ${catkin_LIBRARIES} 94 | ${QT_LIBRARIES} 95 | ${OPENGL_LIBRARIES} 96 | ${GSL_LIBRARIES} 97 | ) 98 | 99 | catkin_install_python (PROGRAMS imagefilter.py 100 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 101 | ) 102 | 103 | 104 | add_executable ( 105 | test_tesselator 106 | test_tesselator.cpp 107 | Tesselator.cpp 108 | vector_map.cpp 109 | ) 110 | 111 | 112 | target_link_libraries ( 113 | test_tesselator 114 | ${OPENGL_LIBRARIES} 115 | boost_system 116 | ) 117 | 118 | 119 | add_executable ( 120 | test_hist 121 | test_hist.cpp 122 | ImageMatch.cpp 123 | RenderWidget.cpp 124 | Pose.cpp 125 | Camera.cpp 126 | ) 127 | 128 | target_link_libraries ( 129 | test_hist 130 | ${OpenCV_LIBRARIES} 131 | boost_system 132 | ${QT_LIBRARIES} 133 | ${OPENGL_LIBRARIES} 134 | ) 135 | 136 | 137 | add_executable ( 138 | solvertest 139 | solvertest.cpp 140 | PointSolver2.cpp 141 | Camera.cpp 142 | ) 143 | 144 | 145 | target_link_libraries ( 146 | solvertest 147 | ${OpenCV_LIBRARIES} 148 | boost_system 149 | ) -------------------------------------------------------------------------------- /vectormap_slam/Camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Camera.h 3 | * 4 | * Created on: Apr 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SRC_CAMERA_H_ 9 | #define SRC_CAMERA_H_ 10 | 11 | 12 | #include "Math.h" 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | class Camera { 19 | public: 20 | Camera (int windowWidth=0, int windowHeight=0); 21 | virtual ~Camera(); 22 | 23 | void setSize (int w, int h); 24 | 25 | inline int getWidth () { return imageWidth; } 26 | inline int getHeight () { return imageHeight; } 27 | 28 | inline Matrix4 &getViewMatrix() 29 | { return viewMatrix; } 30 | 31 | inline Matrix4 &getProjectionMatrix() 32 | { return projectionMatrix; } 33 | 34 | // These functions change View Matrix 35 | void lookAt (const Point3 &eyepos, const Point3 ¢erOfView, const Eigen::Vector3f &up); 36 | void lookAt (const Quaternion &orientation, const Point3 &location); 37 | void lookAt (const tf::Transform &t); 38 | // --- 39 | 40 | // These functions change Projection Matrix 41 | void perspective (float fovy, float aspectRatio, float zNear, float zFar); 42 | void projectionMatrixFromCameraInfo (float fx, float fy, int imageWidth, int imageHeight, float cx, float cy); 43 | void projectionMatrixFromCameraInfo (sensor_msgs::CameraInfo::ConstPtr info); 44 | // --- 45 | 46 | // Playing with view & projection matrices 47 | inline Point3 project (const Point3 &pt) 48 | { 49 | Point4 p4 (pt.x(), pt.y(), pt.z(), 1); 50 | p4 = projectionMatrix * viewMatrix * p4; 51 | return Point3 (p4.x(), p4.y(), p4.z()) / p4.w(); 52 | } 53 | 54 | bool project (const Point3 &pt, int &u, int &v, int reqImageWidth=0, int reqImageHeight=0, bool useOpenGL=true); 55 | 56 | // Transform a point to camera coordinate 57 | inline Point3 transform (const Point3 &pt) 58 | { 59 | Point4 p4 (pt.x(), pt.y(), pt.z(), 1); 60 | p4 = viewMatrix * p4; 61 | return Point3 (p4.x(), p4.y(), p4.z()) / p4.w(); 62 | } 63 | 64 | inline static Point3 transform (const Point3 &pt, tf::StampedTransform &tfsource) 65 | { 66 | tf::Vector3 pt3 (pt.x(), pt.y(), pt.z()); 67 | tf::Vector3 pt3s = tfsource * pt3; 68 | return Point3 (pt3s.x(), pt3s.y(), pt3s.z()); 69 | } 70 | 71 | 72 | // directly mess up with matrices 73 | void setViewMatrix (const Matrix4 &vm) 74 | { viewMatrix = vm; } 75 | 76 | 77 | void setProjectionMatrix (const Matrix4 &pm); 78 | 79 | 80 | struct CameraIntrinsic { 81 | float fx, fy, 82 | wi, wh, 83 | cx, cy; 84 | }; 85 | 86 | 87 | CameraIntrinsic &getCameraParam () { return cameraParams; } 88 | 89 | void getPose (Point3 &pos, Quaternion &orientation); 90 | 91 | 92 | private: 93 | Point3 currentPosition; 94 | Quaternion currentOrientation; 95 | 96 | Matrix4 viewMatrix, 97 | projectionMatrix; 98 | 99 | int imageWidth, imageHeight; 100 | 101 | struct { 102 | float fovy, aspectRatio, zNear, zFar; 103 | bool isPerpective; 104 | } _perspective; 105 | 106 | CameraIntrinsic cameraParams; 107 | }; 108 | 109 | #endif /* SRC_CAMERA_H_ */ 110 | -------------------------------------------------------------------------------- /vectormap_slam/CameraDisplay.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CameraDisplay.cpp 3 | * 4 | * Created on: Jul 27, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "CameraDisplay.h" 9 | #include "RenderWidget.h" 10 | #include "debug.h" 11 | 12 | 13 | 14 | CameraDisplay::CameraDisplay (RenderWidget *w, int width, int height, QWidget *parent) : 15 | glcanvas (w), 16 | rect (width, height) 17 | { 18 | QGLWidget (parent, glcanvas); 19 | if (this->isSharing()) 20 | debug ("Not sharing context"); 21 | setFixedSize (width, height); 22 | } 23 | 24 | 25 | void CameraDisplay::initializeGL() 26 | { 27 | if (glcanvas->context() != this->context()) 28 | debug ("Context not same"); 29 | } 30 | 31 | 32 | void CameraDisplay::update(const cv::Mat &newimg) 33 | { 34 | source = newimg; 35 | this->draw (); 36 | } 37 | 38 | 39 | void CameraDisplay::paintGL() 40 | { 41 | this->draw (); 42 | } 43 | 44 | 45 | void CameraDisplay::draw () 46 | { 47 | if (source.rows==0) 48 | return; 49 | 50 | cv::Mat fbclone = source.clone(); 51 | cv::flip (fbclone, fbclone, -1); 52 | cv::flip (fbclone, fbclone, 1); 53 | 54 | makeCurrent (); 55 | glClear (GL_COLOR_BUFFER_BIT); 56 | glDrawPixels (rect.width(), rect.height(), GL_RGB, GL_UNSIGNED_BYTE, fbclone.data); 57 | // xxx: copy from RenderWidget 58 | QRect rsrc (0, 0, rect.width(), rect.height()); 59 | QGLFramebufferObject::blitFramebuffer(0, rsrc, glcanvas->getFramebuffer(), rsrc); 60 | swapBuffers (); 61 | doneCurrent (); 62 | } 63 | -------------------------------------------------------------------------------- /vectormap_slam/CameraDisplay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * CameraDisplay.h 3 | * 4 | * Created on: Jul 27, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef CAMERADISPLAY_H_ 9 | #define CAMERADISPLAY_H_ 10 | 11 | #include 12 | #include 13 | 14 | 15 | class RenderWidget; 16 | 17 | 18 | class CameraDisplay: public QGLWidget 19 | { 20 | Q_OBJECT 21 | 22 | public: 23 | CameraDisplay (RenderWidget *w, int width, int height, QWidget *parent=NULL); 24 | 25 | void initializeGL (); 26 | void paintGL (); 27 | void update (const cv::Mat &sourceImage); 28 | 29 | private: 30 | RenderWidget *glcanvas; 31 | void draw (); 32 | 33 | cv::Mat source; 34 | }; 35 | 36 | 37 | #endif /* CAMERADISPLAY_H_ */ 38 | -------------------------------------------------------------------------------- /vectormap_slam/Crosswalk.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | *Crosswalk.cpp 3 | * 4 | * Created on: Jul 30, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "Crosswalk.h" 9 | 10 | Crosswalk::Crosswalk() { 11 | // TODO Auto-generated constructor stub 12 | 13 | } 14 | 15 | Crosswalk::~Crosswalk() { 16 | // TODO Auto-generated destructor stub 17 | } 18 | 19 | -------------------------------------------------------------------------------- /vectormap_slam/Crosswalk.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Crosswalk.h 3 | * 4 | * Created on: Jul 30, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _CROSSWALK_H_ 9 | #define _CROSSWALK_H_ 10 | 11 | #include "DrawObjects.h" 12 | 13 | class Crosswalk: public VectorMapObject { 14 | public: 15 | Crosswalk(); 16 | virtual ~Crosswalk(); 17 | }; 18 | 19 | #endif /* _CROSSWALK_H_ */ 20 | -------------------------------------------------------------------------------- /vectormap_slam/DirectionArrow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DirectionArrow.cpp 3 | * 4 | * Created on: Jul 25, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "DirectionArrow.h" 9 | #include "RenderWidget.h" 10 | #include 11 | #include 12 | 13 | 14 | 15 | #define arrowSize 2 16 | Point3 Arrows[arrowSize]; 17 | 18 | 19 | DirectionArrow::DirectionArrow (RenderWidget *w) : 20 | VectorMapObject (w) 21 | { 22 | setColor (255, 0, 0); 23 | } 24 | 25 | 26 | void DirectionArrow::initialize() 27 | { 28 | initBuffer (); 29 | vbuffer->bind(); 30 | vbuffer->allocate(sizeof(Arrows)); 31 | vbuffer->release(); 32 | } 33 | 34 | 35 | void DirectionArrow::draw() 36 | { 37 | vbuffer->bind(); 38 | 39 | Arrows[0] = glwidget->pose()->transformCameraToWorld(Point3 (0, 1, 3)); 40 | Arrows[1] = glwidget->pose()->transformCameraToWorld(Point3 (0, 1, 10)); 41 | 42 | 43 | vbuffer->write(0, Arrows, sizeof(Arrows)); 44 | 45 | glEnableClientState (GL_VERTEX_ARRAY); 46 | glVertexPointer (3, GL_FLOAT, 0, 0); 47 | 48 | //glLineWidth (6); 49 | glPointSize (6); 50 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 51 | glDrawArrays (GL_POINTS, 0, arrowSize); 52 | //glFlush (); 53 | 54 | glDisableClientState (GL_VERTEX_ARRAY); 55 | 56 | vbuffer->release(); 57 | } 58 | -------------------------------------------------------------------------------- /vectormap_slam/DirectionArrow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * DirectionArrow.h 3 | * 4 | * Created on: Jul 25, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef DIRECTIONARROW_H_ 9 | #define DIRECTIONARROW_H_ 10 | 11 | #include 12 | 13 | 14 | class QGLShaderProgram; 15 | 16 | 17 | class DirectionArrow: public VectorMapObject 18 | { 19 | public: 20 | DirectionArrow(RenderWidget *w); 21 | 22 | void initialize (); 23 | void draw (); 24 | 25 | }; 26 | 27 | #endif /* DIRECTIONARROW_H_ */ 28 | -------------------------------------------------------------------------------- /vectormap_slam/DrawObjects.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapObjects.cpp 3 | * 4 | * Created on: Jul 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "DrawObjects.h" 9 | 10 | #include "RenderWidget.h" 11 | #define GLDEBUG 12 | #include "debug.h" 13 | #include 14 | #include 15 | 16 | 17 | using std::vector; 18 | 19 | vector mapPointDrawList; 20 | 21 | 22 | VectorMapObject::VectorMapObject (RenderWidget *w) : 23 | glwidget(w) 24 | {} 25 | 26 | 27 | void VectorMapObject::initBuffer() 28 | { 29 | vbuffer = new QGLBuffer (QGLBuffer::VertexBuffer); 30 | vbuffer->bind(); 31 | vbuffer->setUsagePattern(QGLBuffer::StaticDraw); 32 | bool t = vbuffer->create(); 33 | if (!t) 34 | debug ("Unable to create vertex buffer"); 35 | vbuffer->release(); 36 | } 37 | 38 | 39 | VectorMapObject::~VectorMapObject() 40 | { 41 | vbuffer->destroy(); 42 | delete (vbuffer); 43 | } 44 | 45 | 46 | void PointList::initialize() 47 | { 48 | initBuffer (); 49 | vbuffer->bind(); 50 | int len = pointsrc.size(); 51 | vbuffer->allocate(pointsrc.data(), sizeof(Point3)*len); 52 | //glBufferData (GL_ARRAY_BUFFER, sizeof(Point3)*len, pointsrc.data(), GL_STATIC_DRAW); 53 | vbuffer->release(); 54 | } 55 | 56 | 57 | void PointList::draw() 58 | { 59 | vbuffer->bind(); 60 | glEnableClientState (GL_VERTEX_ARRAY); 61 | glVertexPointer (3, GL_FLOAT, 0, 0); 62 | 63 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 64 | glPointSize (size); 65 | glDrawArrays (GL_POINTS, 0, pointsrc.size()); 66 | 67 | glDisableClientState (GL_VERTEX_ARRAY); 68 | vbuffer->release(); 69 | } 70 | 71 | 72 | TRectangle::TRectangle ( 73 | const Point3 &p1, 74 | const Point3 &p2, 75 | const Point3 &p3, 76 | const Point3 &p4, 77 | RenderWidget *w) : 78 | VectorMapObject (w) 79 | { 80 | points[0] = p1; 81 | points[1] = p2; 82 | points[2] = p3; 83 | points[3] = p4; 84 | color = Vector4 (1.0, 1.0, 1.0, 1.0); 85 | } 86 | 87 | 88 | void TRectangle::initialize() 89 | { 90 | initBuffer (); 91 | vbuffer->bind(); 92 | 93 | const int sz = sizeof(points); 94 | vbuffer->allocate(points, sz); 95 | vbuffer->release(); 96 | } 97 | 98 | 99 | void TRectangle::draw() 100 | { 101 | vbuffer->bind (); 102 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 103 | glEnableClientState (GL_VERTEX_ARRAY); 104 | glVertexPointer (3, GL_FLOAT, 0, 0); 105 | glDrawArrays (GL_TRIANGLE_FAN, 0, 4); 106 | glDisableClientState (GL_VERTEX_ARRAY); 107 | vbuffer->release (); 108 | } 109 | 110 | 111 | 112 | #define QT_NO_KEYWORDS 113 | #undef signals 114 | #include "vector_map.h" 115 | 116 | 117 | MapPoints::MapPoints (VectorMap *mapsrc, RenderWidget *w) : 118 | PointList(w), 119 | map (mapsrc), 120 | filterDistance (false) 121 | { 122 | for (int i=1; i<=map->points.size(); i++) { 123 | Point3 p3 = map->getPoint(i); 124 | pointsrc.push_back (p3); 125 | mapPointDrawList.push_back(i-1); 126 | } 127 | } 128 | 129 | 130 | void filterPoints (vector &ptlist, vector &source, Pose *pose, double distance) 131 | { 132 | ptlist.clear(); 133 | for (unsigned int p=0; ptransformWorldToCamera(source[p]); 135 | if (ptcam.z() < 0) 136 | continue; 137 | if (ptcam.norm() > distance*distance) 138 | continue; 139 | ptlist.push_back(p); 140 | } 141 | } 142 | 143 | 144 | void MapPoints::draw() 145 | { 146 | mapPointDrawList.clear(); 147 | if (filterDistance==true) { 148 | filterPoints (mapPointDrawList, pointsrc, glwidget->pose(), 10.0); 149 | } 150 | 151 | else { 152 | for (int i=0; ibind(); 157 | glEnableClientState (GL_VERTEX_ARRAY); 158 | glVertexPointer (3, GL_FLOAT, 0, 0); 159 | 160 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 161 | glPointSize (size); 162 | glDrawElements (GL_POINTS, mapPointDrawList.size(), GL_UNSIGNED_INT, mapPointDrawList.data()); 163 | 164 | glDisableClientState (GL_VERTEX_ARRAY); 165 | vbuffer->release(); 166 | } 167 | 168 | 169 | MapPoints::~MapPoints () 170 | { 171 | delete (map); 172 | } 173 | -------------------------------------------------------------------------------- /vectormap_slam/DrawObjects.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorMapObjects.h 3 | * 4 | * Created on: Jul 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAPOBJECTS_H_ 9 | #define VECTORMAPOBJECTS_H_ 10 | 11 | #include "Math.h" 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | class RenderWidget; 18 | using std::string; 19 | 20 | 21 | class VectorMapObject { 22 | public: 23 | VectorMapObject(RenderWidget *w); 24 | virtual ~VectorMapObject(); 25 | 26 | virtual void initialize() = 0; 27 | virtual void draw() = 0; 28 | 29 | inline void setColor (const Vector3 &c) 30 | { color = Vector4 (c[0], c[1], c[2], 1.0); } 31 | 32 | inline void setColor ( 33 | const float &r, 34 | const float &g, 35 | const float &b) 36 | { setColor (Vector3 (r, g, b)); } 37 | 38 | inline void setColor (int r, int g, int b) 39 | { setColor (Vector3 ((1.0f/255)*r, (1.0f/255)*g, (1.0f/255)*b)); } 40 | 41 | 42 | protected: 43 | Vector4 color; 44 | QGLBuffer *vbuffer; 45 | RenderWidget *glwidget; 46 | void initBuffer (); 47 | }; 48 | 49 | 50 | 51 | #include 52 | using std::vector; 53 | 54 | 55 | class PointList: public VectorMapObject { 56 | public: 57 | PointList (RenderWidget *w) : 58 | VectorMapObject(w), size(1.0) {} 59 | 60 | void initialize (); 61 | void draw (); 62 | 63 | inline void add (const Point3 &p) 64 | { pointsrc.push_back(p); } 65 | 66 | inline void setPointSize (float f) {size=f;} 67 | 68 | protected: 69 | vector pointsrc; 70 | float size; 71 | }; 72 | 73 | 74 | class TRectangle: public VectorMapObject { 75 | public: 76 | TRectangle (const Point3 &p1, const Point3 &p2, const Point3 &p3, const Point3 &p4, RenderWidget *w); 77 | void initialize (); 78 | void draw (); 79 | 80 | private: 81 | Point3 points[4]; 82 | }; 83 | 84 | 85 | 86 | class VectorMap; 87 | 88 | class MapPoints : public PointList 89 | { 90 | public: 91 | MapPoints (VectorMap *mapsrc, RenderWidget *w); 92 | ~MapPoints (); 93 | void draw (); 94 | bool filterDistance; 95 | 96 | protected: 97 | VectorMap *map; 98 | }; 99 | 100 | 101 | inline QVector4D color2qv (const Vector4 &v) 102 | { 103 | return QVector4D (v.x(), v.y(), v.z(), v.w()); 104 | } 105 | 106 | 107 | 108 | #endif /* VECTORMAPOBJECTS_H_ */ 109 | -------------------------------------------------------------------------------- /vectormap_slam/Formulas.wxm: -------------------------------------------------------------------------------- 1 | /* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ 2 | /* [ Created with wxMaxima version 13.04.2 ] */ 3 | 4 | /* [wxMaxima: input start ] */ 5 | P1: [p1x, p1y]; 6 | /* [wxMaxima: input end ] */ 7 | 8 | /* [wxMaxima: input start ] */ 9 | P2: [p2x, p2y]; 10 | /* [wxMaxima: input end ] */ 11 | 12 | /* [wxMaxima: input start ] */ 13 | P: [px, py]; 14 | /* [wxMaxima: input end ] */ 15 | 16 | /* [wxMaxima: input start ] */ 17 | Q: P1 + (P2-P1)*((P-P1) . (P2-P1))/((P2-P1).(P2-P1)); 18 | /* [wxMaxima: input end ] */ 19 | 20 | /* [wxMaxima: input start ] */ 21 | e(P1, P2) := (P-Q) . (P-Q); 22 | /* [wxMaxima: input end ] */ 23 | 24 | /* [wxMaxima: input start ] */ 25 | len: (P2-P1) . (P2-P1); 26 | /* [wxMaxima: input end ] */ 27 | 28 | /* [wxMaxima: input start ] */ 29 | factor( diff (e(P1, P2), p1x) ); 30 | /* [wxMaxima: input end ] */ 31 | 32 | /* [wxMaxima: input start ] */ 33 | factor( diff (e(P1, P2), p1y) ); 34 | /* [wxMaxima: input end ] */ 35 | 36 | /* [wxMaxima: input start ] */ 37 | factor ( diff (e(P1, P2), p2x) ); 38 | /* [wxMaxima: input end ] */ 39 | 40 | /* [wxMaxima: input start ] */ 41 | factor (diff (e(P1, P2), p2y) ); 42 | /* [wxMaxima: input end ] */ 43 | 44 | /* [wxMaxima: input start ] */ 45 | factor (x^2 -2*x+1); 46 | /* [wxMaxima: input end ] */ 47 | 48 | /* Maxima can't load/batch files which end with a comment! */ 49 | "Created with wxMaxima"$ 50 | -------------------------------------------------------------------------------- /vectormap_slam/Gutters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Gutters.cpp 3 | * 4 | * Created on: Aug 5, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "Gutters.h" 9 | #include "vector_map.h" 10 | #include "debug.h" 11 | #include 12 | #include 13 | #include 14 | #include "RenderWidget.h" 15 | 16 | 17 | using std::ifstream; 18 | using std::string; 19 | 20 | 21 | Gutters::Gutters(VectorMap *v, RenderWidget *w) : 22 | VectorMapObject (w), 23 | map (v) 24 | { 25 | setColor (188, 157, 157); 26 | 27 | string fname = map->dirname + "/gutter.csv"; 28 | ifstream gutterfd (fname); 29 | 30 | int lptr = 0; 31 | while (gutterfd.good()) { 32 | string line; 33 | int id, aid, ty, linkId; 34 | getline (gutterfd, line); 35 | sscanf (line.c_str(), "%d,%d,%d,%d", &id, &aid, &ty, &linkId); 36 | if (lptr > 0) { 37 | areaIds.push_back(aid); 38 | } 39 | lptr++; 40 | } 41 | } 42 | 43 | 44 | void Gutters::initialize () 45 | { 46 | initBuffer (); 47 | vbuffer->bind(); 48 | 49 | int pid=0; 50 | for (int i=0; iareas[areaIds[i]]; 52 | vector pointsInArea = areaToPoints (a, *map); 53 | pointBuffer.insert (pointBuffer.end(), pointsInArea.begin(), pointsInArea.end()); 54 | 55 | vector pidxs; 56 | for (uint32_t i=pid; irelease(); 63 | } 64 | 65 | 66 | void Gutters::draw () 67 | { 68 | vbuffer->bind(); 69 | glEnableClientState (GL_VERTEX_ARRAY); 70 | glVertexPointer (3, GL_FLOAT, 0, 0); 71 | 72 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 73 | for (int aid=0; aid cAreaPtIdx = pointIndex[aid]; 75 | glDrawElements (GL_POLYGON, cAreaPtIdx.size(), GL_UNSIGNED_INT, cAreaPtIdx.data()); 76 | } 77 | 78 | glDisableClientState (GL_VERTEX_ARRAY); 79 | vbuffer->release(); 80 | } 81 | -------------------------------------------------------------------------------- /vectormap_slam/Gutters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Gutters.h 3 | * 4 | * Created on: Aug 5, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef GUTTERS_H_ 9 | #define GUTTERS_H_ 10 | 11 | #include "AreaList.h" 12 | 13 | class Gutters: public VectorMapObject 14 | { 15 | public: 16 | Gutters(VectorMap *v, RenderWidget *w); 17 | void initialize (); 18 | void draw (); 19 | 20 | protected: 21 | VectorMap *map; 22 | vector pointBuffer; 23 | vector > pointIndex; 24 | vector areaIds; 25 | }; 26 | 27 | #endif /* GUTTERS_H_ */ 28 | -------------------------------------------------------------------------------- /vectormap_slam/ImageDisplay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageDisplay.h 3 | * 4 | * Created on: Aug 7, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _IMAGEDISPLAY_H_ 9 | #define _IMAGEDISPLAY_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | 17 | class RenderWidget; 18 | 19 | 20 | class ImageDisplay: public QWidget 21 | { 22 | Q_OBJECT 23 | 24 | public: 25 | ImageDisplay (QSize imgsize, RenderWidget *canvas, QWidget *parent=NULL); 26 | virtual ~ImageDisplay(); 27 | 28 | cv::Mat& getImage () { return cameraImage; } 29 | cv::Mat& getGray () { return grayImage; } 30 | cv::Mat& getProcessedGray () { return processedGrayImage; } 31 | cv::Mat& getMask () { return mask; } 32 | 33 | void updateImage (const cv::Mat &src); 34 | 35 | void dump (); 36 | 37 | void paintEvent (QPaintEvent *event); 38 | 39 | enum { 40 | ImageTypeRgb, 41 | ImageTypeGray, 42 | ImageTypeGrayProcessed 43 | }; 44 | 45 | public slots: 46 | void changeImageType (int type); 47 | void toggleMapProjection (bool shown); 48 | 49 | 50 | protected: 51 | QSize mySize; 52 | 53 | cv::Mat cameraImage; 54 | cv::Mat grayImage; 55 | cv::Mat processedGrayImage; 56 | cv::Mat mask; 57 | 58 | QImage *qCameraImage, 59 | *qGrayImage, 60 | *qProcessedGrayImage, 61 | *qMask; 62 | 63 | RenderWidget *glcanvas; 64 | QPixmap *compositeImage; 65 | 66 | int displayType; 67 | 68 | bool mapShown; 69 | 70 | void applyOverlay (); 71 | }; 72 | 73 | #endif /* _IMAGEDISPLAY_H_ */ 74 | -------------------------------------------------------------------------------- /vectormap_slam/ImageMatch.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageMatch.h 3 | * 4 | * Created on: Jul 20, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef IMAGEMATCH_H_ 9 | #define IMAGEMATCH_H_ 10 | 11 | 12 | #include 13 | #include "Math.h" 14 | #include "Pose.h" 15 | #include 16 | 17 | 18 | class QImage; 19 | 20 | 21 | struct PoseOffset { 22 | double x, y, z, roll, pitch, yaw; 23 | }; 24 | 25 | 26 | class RenderWidget; 27 | 28 | 29 | class ImageMatch { 30 | public: 31 | ImageMatch (cv::Mat &imageProcGray, RenderWidget *glc, cv::Mat *mask=NULL); 32 | Point3 find (); 33 | 34 | static double entropy (const cv::Mat &image); 35 | static double entropy (const cv::Mat &image1, const cv::Mat &image2, bool dumpHistogram=false); 36 | static double nmi (const cv::Mat &image1, const cv::Mat &image2); 37 | 38 | /* For simulated annealing */ 39 | static double simanEnergy (void *xp); 40 | static double simanDistance (void *xp, void *xy); 41 | static void simanStep (const gsl_rng *rng, void *xp, double step_size); 42 | static void simanPrintCfg (void *xp); 43 | 44 | private: 45 | RenderWidget *glcanvas; 46 | cv::Mat &cameraRefGray; 47 | cv::Mat *mask; 48 | PoseOffset poseConfig; 49 | 50 | gsl_siman_params_t simanParams; 51 | }; 52 | 53 | void writeArray(std::string name, Eigen::ArrayXXf &src); 54 | 55 | Point3 imageSearch (const cv::Mat &cameraRefGray, RenderWidget *widget, const cv::Mat *mask=NULL); 56 | 57 | void imagePipeline (cv::Mat &imageSource, cv::Mat &target); 58 | 59 | void histogramBinary (const cv::Mat &imageGray, cv::Mat &hist); 60 | void histogramBinary (const cv::Mat &imageGray1, const cv::Mat &imageGray2, cv::Mat &hist); 61 | 62 | #endif /* IMAGEMATCH_H_ */ 63 | -------------------------------------------------------------------------------- /vectormap_slam/Jacobian for Projection from Camera Coordinate.wxm: -------------------------------------------------------------------------------- 1 | /* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ 2 | /* [ Created with wxMaxima version 13.04.2 ] */ 3 | 4 | /* [wxMaxima: input start ] */ 5 | K: matrix( 6 | [fx, 0, cx, 0], 7 | [0, fy, cy, 0], 8 | [0, 0, a, b], 9 | [0, 0, -1, 0] 10 | ); 11 | C: [x, y, z, 1]; 12 | /* [wxMaxima: input end ] */ 13 | 14 | /* [wxMaxima: input start ] */ 15 | Pk: K . C; 16 | Pl: Pk / Pk[4][1]; 17 | Pt: [ 18 | Pl[1][1] / Pl[3][1], 19 | Pl[2][1] / Pl[3][1] 20 | ]; 21 | /* [wxMaxima: input end ] */ 22 | 23 | /* [wxMaxima: input start ] */ 24 | u(p) := ( cx*z(p) + fx*x(p) ) / ( a*z(p) + b ); 25 | v(p) := ( cy*z(p) + fy*y(p) ) / ( a*z(p) + b ); 26 | /* [wxMaxima: input end ] */ 27 | 28 | /* [wxMaxima: input start ] */ 29 | diff (u(p), p); 30 | diff (v(p), p); 31 | /* [wxMaxima: input end ] */ 32 | 33 | /* Maxima can't load/batch files which end with a comment! */ 34 | "Created with wxMaxima"$ 35 | -------------------------------------------------------------------------------- /vectormap_slam/LineList.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LineList.cpp 3 | * 4 | * Created on: Jul 19, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "LineList.h" 9 | #include "RenderWidget.h" 10 | 11 | #define QT_NO_KEYWORDS 12 | #undef signals 13 | #include "vector_map.h" 14 | 15 | 16 | LineList::LineList (VectorMap *vm, double s, RenderWidget *w) : 17 | VectorMapObject (w), 18 | scale(s) 19 | { 20 | for(std::map::iterator it=vm->lines.begin(); it!=vm->lines.end(); it++) { 21 | Line l = it->second; 22 | Point3 p1, p2; 23 | p1.x() = vm->points[l.bpid].bx; 24 | p1.y() = vm->points[l.bpid].ly; 25 | p1.z() = vm->points[l.bpid].h; 26 | p2.x() = vm->points[l.fpid].bx; 27 | p2.y() = vm->points[l.fpid].ly; 28 | p2.z() = vm->points[l.fpid].h; 29 | 30 | Point3 r1, r2, r3, r4; 31 | LineList::createBox (p1, p2, r1, r2, r3, r4, scale); 32 | points.push_back(r1); 33 | points.push_back(r2); 34 | points.push_back(r3); 35 | points.push_back(r3); 36 | points.push_back(r4); 37 | points.push_back(r2); 38 | } 39 | 40 | setColor(0, 255, 0); 41 | } 42 | 43 | 44 | void LineList::initialize() 45 | { 46 | initBuffer (); 47 | vbuffer->bind(); 48 | int len = points.size(); 49 | vbuffer->allocate(points.data(), sizeof(Point3)*len); 50 | vbuffer->release(); 51 | } 52 | 53 | 54 | void LineList::draw() 55 | { 56 | vbuffer->bind(); 57 | glEnableClientState (GL_VERTEX_ARRAY); 58 | glVertexPointer (3, GL_FLOAT, 0, 0); 59 | 60 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 61 | glDrawArrays (GL_TRIANGLES, 0, points.size()); 62 | // glLineWidth (1.0); 63 | // glDrawArrays (GL_LINES, 0, points.size()); 64 | //glFlush (); 65 | 66 | glDisableClientState (GL_VERTEX_ARRAY); 67 | vbuffer->release(); 68 | } 69 | 70 | 71 | Point2 Point3to2 (const Point3 &p3) 72 | { return Point2 (p3.x(), p3.y()); } 73 | 74 | 75 | Point3 Point2to3 (const Point2 &p2, double zval) 76 | { return Point3 (p2.x(), p2.y(), zval); } 77 | 78 | 79 | void LineList::createBox ( 80 | const Point2 &source1, 81 | const Point2 &source2, 82 | Point2 &result1, 83 | Point2 &result2, 84 | Point2 &result3, 85 | Point2 &result4, 86 | double scale) 87 | { 88 | Vector2 u = source2 - source1; 89 | u.normalize(); 90 | u = u * scale/2; 91 | Point2 s1 = source1 - u, 92 | s2 = source2 + u; 93 | Vector2 v = perpendicular (u); 94 | v.normalize(); 95 | v = v * scale/2; 96 | result1 = s1 - v; 97 | result2 = s1 + v; 98 | result3 = s2 - v; 99 | result4 = s2 + v; 100 | } 101 | 102 | 103 | void LineList::createBox ( 104 | const Point3 &source1, 105 | const Point3 &source2, 106 | Point3 &result1, 107 | Point3 &result2, 108 | Point3 &result3, 109 | Point3 &result4, 110 | double scale) 111 | { 112 | Point2 s1 = Point3to2 (source1), 113 | s2 = Point3to2 (source2); 114 | Point2 r1, r2, r3, r4; 115 | 116 | createBox (s1, s2, r1, r2, r3, r4, scale); 117 | result1 = Point2to3 (r1, source1.z()); 118 | result2 = Point2to3 (r2, source1.z()); 119 | result3 = Point2to3 (r3, source2.z()); 120 | result4 = Point2to3 (r4, source2.z()); 121 | } 122 | -------------------------------------------------------------------------------- /vectormap_slam/LineList.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LineList.h 3 | * 4 | * Created on: Jul 19, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAP_SLAM_LINELIST_H_ 9 | #define VECTORMAP_SLAM_LINELIST_H_ 10 | 11 | #include "DrawObjects.h" 12 | #include "Math.h" 13 | #include 14 | 15 | 16 | using std::vector; 17 | class VectorMap; 18 | 19 | 20 | class LineList: public VectorMapObject 21 | { 22 | public: 23 | LineList (VectorMap *v, double scale, RenderWidget *w); 24 | 25 | void initialize (); 26 | void draw (); 27 | 28 | static void createBox ( 29 | const Point3 &source1, 30 | const Point3 &source2, 31 | Point3 &result1, 32 | Point3 &result2, 33 | Point3 &result3, 34 | Point3 &result4, 35 | double scale); 36 | 37 | static void createBox ( 38 | const Point2 &source1, 39 | const Point2 &source2, 40 | Point2 &result1, 41 | Point2 &result2, 42 | Point2 &result3, 43 | Point2 &result4, 44 | double scale); 45 | 46 | 47 | protected: 48 | vector points; 49 | double scale; 50 | }; 51 | 52 | #endif /* VECTORMAP_SLAM_LINELIST_H_ */ 53 | -------------------------------------------------------------------------------- /vectormap_slam/LineRenderer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LineRenderer.cpp 3 | * 4 | * Created on: Sep 1, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "LineRenderer.h" 9 | 10 | LineRenderer::LineRenderer() { 11 | // TODO Auto-generated constructor stub 12 | 13 | } 14 | 15 | -------------------------------------------------------------------------------- /vectormap_slam/LineRenderer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LineRenderer.h 3 | * 4 | * Created on: Sep 1, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _LINERENDERER_H_ 9 | #define _LINERENDERER_H_ 10 | 11 | 12 | #include 13 | #include "vector_map.h" 14 | #include "Camera.h" 15 | 16 | 17 | using std::vector; 18 | 19 | 20 | class LineRenderer 21 | { 22 | public: 23 | LineRenderer (VectorMap *, Camera *); 24 | 25 | void render (Point3 &position, Quaternion &orientation, vector &lines); 26 | 27 | protected: 28 | VectorMap *map; 29 | }; 30 | 31 | #endif /* _LINERENDERER_H_ */ 32 | -------------------------------------------------------------------------------- /vectormap_slam/LinesRaw.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LinesRaw.cpp 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "LinesRaw.h" 9 | #include "RenderWidget.h" 10 | 11 | 12 | LinesRaw::LinesRaw (VectorMap *vm, RenderWidget *w) : 13 | VectorMapObject (w) 14 | { 15 | for(std::map::iterator it=vm->lines.begin(); it!=vm->lines.end(); it++) { 16 | Line l = it->second; 17 | int pid1 = l.bpid, pid2 = l.fpid; 18 | Point3 p1 = vm->getPoint(pid1), 19 | p2 = vm->getPoint(pid2); 20 | points.push_back(p1); 21 | points.push_back(p2); 22 | } 23 | 24 | setColor(0, 255, 0); 25 | } 26 | 27 | 28 | void LinesRaw::initialize() 29 | { 30 | initBuffer (); 31 | vbuffer->bind(); 32 | int len = points.size(); 33 | vbuffer->allocate(points.data(), sizeof(Point3)*len); 34 | vbuffer->release(); 35 | } 36 | 37 | 38 | void LinesRaw::draw() 39 | { 40 | vbuffer->bind(); 41 | glEnableClientState (GL_VERTEX_ARRAY); 42 | glVertexPointer (3, GL_FLOAT, 0, 0); 43 | 44 | glLineWidth (1.0); 45 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 46 | glDrawArrays (GL_LINES, 0, points.size()); 47 | 48 | glDisableClientState (GL_VERTEX_ARRAY); 49 | vbuffer->release(); 50 | } 51 | -------------------------------------------------------------------------------- /vectormap_slam/LinesRaw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LinesRaw.h 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _LINESRAW_H_ 9 | #define _LINESRAW_H_ 10 | 11 | #include "DrawObjects.h" 12 | #include 13 | #include "Math.h" 14 | #include "vector_map.h" 15 | 16 | 17 | 18 | class LinesRaw: public VectorMapObject { 19 | public: 20 | LinesRaw (VectorMap *v, RenderWidget *w); 21 | 22 | void initialize (); 23 | void draw (); 24 | 25 | protected: 26 | vector points; 27 | 28 | }; 29 | 30 | #endif /* _LINESRAW_H_ */ 31 | -------------------------------------------------------------------------------- /vectormap_slam/MatchWindow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MatchWindow.h 3 | * 4 | * Created on: Jul 6, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef MATCHWINDOW_H_ 9 | #define MATCHWINDOW_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "Math.h" 23 | 24 | 25 | 26 | using std::string; 27 | 28 | 29 | 30 | class LabelWithTextBox: public QWidget 31 | { 32 | Q_OBJECT 33 | public: 34 | LabelWithTextBox (const string &textlabel, QWidget *parent=NULL); 35 | void setText (const string &text) 36 | { textEntry->setText(QString::fromStdString(text)); } 37 | void setText (const QString &txt) 38 | { textEntry->setText (txt); } 39 | void setText (const double &num) 40 | { 41 | QString t = QString::number(num); 42 | textEntry->setText(t); 43 | } 44 | 45 | public slots: 46 | void plusClick (); 47 | void minusClick (); 48 | signals: 49 | void doPoseChange (QString buttonGroupName, int btnType); 50 | 51 | 52 | protected: 53 | QLineEdit *textEntry; 54 | QPushButton *plus, *minus; 55 | string text; 56 | }; 57 | 58 | 59 | static const float 60 | dX = 0.1, 61 | dY = 0.1, 62 | dZ = 0.1, 63 | dR = M_PI/180.0, 64 | dP = M_PI/180.0, 65 | dYw = M_PI/180.0; 66 | 67 | 68 | class RenderWidget; 69 | class ImageDisplay; 70 | class VectorMap; 71 | class PointSolver; 72 | 73 | 74 | class MatchWindow: public QMainWindow { 75 | Q_OBJECT 76 | 77 | public: 78 | MatchWindow (); 79 | virtual ~MatchWindow(); 80 | 81 | void updatePoseText (); 82 | void setImageSource (const cv::Mat &image); 83 | 84 | bool isClosed () 85 | { return closed; } 86 | int getImageWidth () 87 | { return defaultImageWidth; } 88 | int getImageHeight () 89 | { return defaultImageHeight; } 90 | 91 | void setPose (const Point3 &position, const Quaternion &orientation); 92 | 93 | void update (); 94 | 95 | RenderWidget *canvas() { return glcanvas; } 96 | ImageDisplay *display() { return imageDisplay; } 97 | 98 | /* XXX: Please keep aspect ratio to be 4/3 */ 99 | static const int defaultImageWidth = 640, 100 | defaultImageHeight = 480; 101 | 102 | enum PoseTuneButton { 103 | Plus, Minus 104 | }; 105 | 106 | 107 | signals: 108 | 109 | public slots: 110 | void captureClicked (void); 111 | void PoseChangeManual (QString btnId, int btn); 112 | void searchButtonClicked (void); 113 | void resetButtonClicked (void); 114 | 115 | 116 | protected: 117 | QWidget *centralWidget; 118 | QVBoxLayout *mainLayout; 119 | 120 | QWidget *imagesContainer; 121 | 122 | ImageDisplay *imageDisplay; 123 | RenderWidget *glcanvas; 124 | 125 | // position & orientation 126 | LabelWithTextBox *wposx, *wposy, *wposz; 127 | LabelWithTextBox *wroll, *wpitch, *wyaw; 128 | 129 | bool closed; 130 | void closeEvent (QCloseEvent *event); 131 | 132 | //sensor_msgs::CameraInfo cameraInfo; 133 | 134 | VectorMap *vmap; 135 | PointSolver *cameraFix; 136 | }; 137 | 138 | 139 | 140 | #endif /* MATCHWINDOW_H_ */ 141 | -------------------------------------------------------------------------------- /vectormap_slam/MatchWindow.h.bak: -------------------------------------------------------------------------------- 1 | /* 2 | * MatchWindow.h 3 | * 4 | * Created on: Jul 6, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef MATCHWINDOW_H_ 9 | #define MATCHWINDOW_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "Math.h" 23 | 24 | 25 | 26 | using std::string; 27 | 28 | 29 | 30 | class LabelWithTextBox: public QWidget 31 | { 32 | Q_OBJECT 33 | public: 34 | LabelWithTextBox (const string &textlabel, QWidget *parent=NULL); 35 | void setText (const string &text) 36 | { textEntry->setText(QString::fromStdString(text)); } 37 | void setText (const QString &txt) 38 | { textEntry->setText (txt); } 39 | void setText (const double &num) 40 | { 41 | QString t = QString::number(num); 42 | textEntry->setText(t); 43 | } 44 | 45 | public slots: 46 | void plusClick (); 47 | void minusClick (); 48 | signals: 49 | void doPoseChange (QString buttonGroupName, int btnType); 50 | 51 | 52 | protected: 53 | QLineEdit *textEntry; 54 | QPushButton *plus, *minus; 55 | string text; 56 | }; 57 | 58 | 59 | static const float 60 | dX = 0.1, 61 | dY = 0.1, 62 | dZ = 0.1, 63 | dR = M_PI/180.0, 64 | dP = M_PI/180.0, 65 | dYw = M_PI/180.0; 66 | 67 | 68 | class RenderWidget; 69 | 70 | 71 | class MatchWindow: public QMainWindow { 72 | Q_OBJECT 73 | 74 | public: 75 | MatchWindow (); 76 | virtual ~MatchWindow(); 77 | 78 | void updatePoseText (); 79 | void setImageSource (const cv::Mat &image); 80 | 81 | bool isClosed () 82 | { return closed; } 83 | int getImageWidth () 84 | { return imagebmp->width(); } 85 | int getImageHeight () 86 | { return imagebmp->height(); } 87 | 88 | void setPose (const Point3 &position, const Quaternion &orientation); 89 | 90 | void update (); 91 | 92 | RenderWidget *canvas() { return glcanvas; } 93 | 94 | /* XXX: Please keep aspect ratio to be 4/3 */ 95 | static const int defaultImageWidth = 1024, 96 | defaultImageHeight = 768; 97 | 98 | enum PoseTuneButton { 99 | Plus, Minus 100 | }; 101 | 102 | 103 | signals: 104 | 105 | public slots: 106 | void captureClicked (void); 107 | void PoseChangeManual (QString btnId, int btn); 108 | void searchButtonClicked (void); 109 | void resetButtonClicked (void); 110 | 111 | 112 | protected: 113 | QWidget *centralWidget; 114 | QVBoxLayout *mainLayout; 115 | 116 | QWidget *imagesContainer; 117 | 118 | QLabel *imageSrc; 119 | QImage *imagebmp; 120 | RenderWidget *glcanvas; 121 | //CameraMapOverlay *glImage; 122 | //QLabel *glImage; 123 | bool imageSrcIsSet; 124 | 125 | // position & orientation 126 | LabelWithTextBox *wposx, *wposy, *wposz; 127 | LabelWithTextBox *wroll, *wpitch, *wyaw; 128 | 129 | bool closed; 130 | void closeEvent (QCloseEvent *event); 131 | 132 | cv::Mat mainImage; 133 | cv::Mat grayImage; 134 | 135 | //sensor_msgs::CameraInfo cameraInfo; 136 | void applyOverlay (); 137 | }; 138 | 139 | 140 | 141 | #endif /* MATCHWINDOW_H_ */ 142 | -------------------------------------------------------------------------------- /vectormap_slam/Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Math.h 3 | * 4 | * Created on: Apr 8, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef SRC_MATH_H_ 9 | #define SRC_MATH_H_ 10 | 11 | #include 12 | #include 13 | 14 | 15 | typedef Eigen::Vector2f Point2; 16 | typedef Eigen::Vector3f Point3; 17 | typedef Eigen::Vector3f Vector3; 18 | typedef Eigen::Vector4f Point4; 19 | typedef Eigen::Vector4f Vector4; 20 | typedef Eigen::Vector2f Vector2; 21 | typedef Eigen::Quaternionf Quaternion; 22 | typedef Eigen::Matrix4f Matrix4; 23 | typedef Eigen::Matrix2f Matrix2; 24 | typedef Eigen::Matrix3f Matrix3; 25 | 26 | 27 | template static F degreeToRadian (const F degree) 28 | { return degree * M_PI / 180; } 29 | 30 | template static F radianToDegree (const F radian) 31 | { return radian * 180 / M_PI; } 32 | 33 | 34 | inline double distance (const Point2 &p1, const Point2 &p2) 35 | { 36 | Vector2 p3 (p2.x()-p1.x(), p2.y()-p1.y()); 37 | return sqrt (p3.squaredNorm()); 38 | } 39 | 40 | 41 | inline double distance (const Point3 &p1, const Point3 &p2) 42 | { 43 | Vector3 p3 (p2.x()-p1.x(), p2.y()-p1.y(), p2.z()-p1.z()); 44 | return sqrt(p3.squaredNorm()); 45 | } 46 | 47 | 48 | inline double distance (const Point4 &p1, const Point4 &p2) 49 | { 50 | Vector4 p3 (p2.x()-p1.x(), p2.y()-p1.y(), p2.z()-p1.z(), p2.w()-p1.w()); 51 | return sqrt(p3.squaredNorm()); 52 | } 53 | 54 | 55 | inline double distance (const double &x1, const double &y1, const double &x2, const double &y2) 56 | { 57 | Point2 p(x1, y1), q(x2, y2); 58 | return distance (p, q); 59 | } 60 | 61 | 62 | inline Vector2 perpendicular (const Vector2 &v) 63 | { return Vector2 (-v.y(), v.x()); } 64 | 65 | 66 | inline void rotate (Vector2 &v, const float angleInDegree) 67 | { 68 | Matrix2 rot; 69 | double c = cos (degreeToRadian(angleInDegree)), 70 | s = sin (degreeToRadian(angleInDegree)); 71 | rot (0, 0) = c; 72 | rot (0, 1) = -s; 73 | rot (1, 0) = s; 74 | rot (1, 1) = c; 75 | v = rot * v; 76 | } 77 | 78 | 79 | #endif /* SRC_MATH_H_ */ 80 | -------------------------------------------------------------------------------- /vectormap_slam/PointSolver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PointSolver.h 3 | * 4 | * Created on: Aug 27, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef POINTSOLVER_H_ 9 | #define POINTSOLVER_H_ 10 | 11 | 12 | #include "vector_map.h" 13 | #include "Math.h" 14 | #include 15 | #include 16 | 17 | 18 | 19 | using std::vector; 20 | class RenderWidget; 21 | 22 | typedef float pscalar; 23 | 24 | 25 | 26 | class PointSolver 27 | { 28 | public: 29 | PointSolver (VectorMap *src, RenderWidget *gl, int targetWidth, int targetHeight); 30 | 31 | // XXX: requires output 32 | void solve (cv::Mat *processedInputImage, Point3 &startPosition, Quaternion &startOrientation); 33 | 34 | 35 | struct ImagePoint { 36 | float px, py; 37 | int nearestLine; 38 | pscalar lineDistance; 39 | }; 40 | 41 | 42 | struct ProjectedPoint { 43 | Point2 coord; 44 | int mapPid; 45 | pscalar jacobian[7][2]; 46 | }; 47 | 48 | 49 | struct LineSegment2D { 50 | ProjectedPoint A, B; 51 | int mapLid; 52 | 53 | pscalar error (Point2 &p); 54 | pscalar errorJacobian (Point2 &p, pscalar jacobianMat[7]); 55 | 56 | pscalar lengthSquared () 57 | { return (B.coord - A.coord).squaredNorm(); } 58 | 59 | pscalar length () 60 | { return (B.coord - A.coord).norm(); } 61 | 62 | pscalar distanceSquared (Point2 &p); 63 | }; 64 | 65 | 66 | private: 67 | VectorMap *map; 68 | RenderWidget *glcanvas; 69 | 70 | cv::Mat *image; 71 | Point3 position0; 72 | Quaternion orientation0; 73 | int width, height; 74 | 75 | // this vector is a list of all white points in source image 76 | vector imagePoints; 77 | // list of all lines currently visible when projected in image 78 | vector visibleLines; 79 | 80 | void projectLines (); 81 | void prepareImage (); 82 | void prepareMatrices (); 83 | void solveForCorrection (); 84 | 85 | // for debugging only 86 | void debugProjection (vector &projResult); 87 | void debugPointLinePair (); 88 | 89 | // solution matrices 90 | Eigen::MatrixXd Jac; 91 | Eigen::VectorXd Pcorrect, pointErrs; 92 | }; 93 | 94 | #endif /* POINTSOLVER_H_ */ 95 | -------------------------------------------------------------------------------- /vectormap_slam/PolesDraw.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PolesDraw.cpp 3 | * 4 | * Created on: Jul 25, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | #include "vector_map.h" 10 | #include "RenderWidget.h" 11 | 12 | 13 | PolesDraw::PolesDraw (VectorMap *vmap, RenderWidget *w) : 14 | VectorMapObject (w) 15 | { 16 | for(std::map::iterator it=vmap->poles.begin(); it!=vmap->poles.end(); it++) { 17 | Pole cpole = it->second; 18 | Point3 pbottom = vmap->getPoint(vmap->vectors[cpole.vid].pid); 19 | Point3 ptop = pbottom + Vector3 (0, 0, cpole.length); 20 | 21 | polePoints.push_back(pbottom); 22 | polePoints.push_back(ptop); 23 | } 24 | 25 | setColor(150, 150, 150); 26 | } 27 | 28 | 29 | void PolesDraw::initialize() 30 | { 31 | initBuffer (); 32 | vbuffer->bind(); 33 | vbuffer->allocate(polePoints.data(), sizeof(Point3)*polePoints.size()); 34 | vbuffer->release(); 35 | } 36 | 37 | 38 | void PolesDraw::draw() 39 | { 40 | vbuffer->bind(); 41 | glEnableClientState (GL_VERTEX_ARRAY); 42 | glVertexPointer (3, GL_FLOAT, 0, 0); 43 | 44 | glLineWidth (3.0); 45 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 46 | glDrawArrays (GL_LINES, 0, polePoints.size()); 47 | //glFlush (); 48 | 49 | glDisableClientState (GL_VERTEX_ARRAY); 50 | vbuffer->release(); 51 | } 52 | -------------------------------------------------------------------------------- /vectormap_slam/PolesDraw.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PolesDraw.h 3 | * 4 | * Created on: Jul 25, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef VECTORMAP_SLAM_POLESDRAW_H_ 9 | #define VECTORMAP_SLAM_POLESDRAW_H_ 10 | 11 | #include 12 | #include 13 | 14 | 15 | using std::vector; 16 | 17 | 18 | class PolesDraw: public VectorMapObject 19 | { 20 | public: 21 | PolesDraw(VectorMap *v, RenderWidget *w); 22 | 23 | void initialize (); 24 | void draw (); 25 | 26 | protected: 27 | vector polePoints; 28 | }; 29 | 30 | #endif /* VECTORMAP_SLAM_POLESDRAW_H_ */ 31 | -------------------------------------------------------------------------------- /vectormap_slam/Pose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Pose.cpp 3 | * 4 | * Created on: Jul 28, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "Pose.h" 9 | #include "tf_eigen.h" 10 | 11 | 12 | void Pose::setRotationOffset (const Quaternion &rot) 13 | { 14 | tf::Transform noff = eigen2tf (rot, Point3(0, 0, 0)); 15 | tf::Transform coff = eigen2tf (orientationOffset, positionOffset); 16 | tf::Transform offset = noff * coff; 17 | tf2eigen (offset, orientationOffset, positionOffset); 18 | } 19 | 20 | 21 | void Pose::offset (double x, double y, double z, double roll, double pitch, double yaw) 22 | { 23 | Point3 ofp (x, y, z); 24 | offset (ofp); 25 | offsetRoll (roll); 26 | offsetPitch (pitch); 27 | offsetYaw (yaw); 28 | } 29 | 30 | 31 | void Pose::offsetRoll(double roll) 32 | { 33 | Quaternion rot (cos(roll/2), 0, 0, sin(roll/2)); 34 | setRotationOffset (rot); 35 | } 36 | 37 | 38 | void Pose::offsetPitch(double pitch) 39 | { 40 | Quaternion rot (cos(pitch/2), sin(pitch/2), 0, 0); 41 | setRotationOffset (rot); 42 | } 43 | 44 | 45 | void Pose::offsetYaw(double yaw) 46 | { 47 | Quaternion rot (cos(yaw/2), 0, sin(yaw/2), 0); 48 | setRotationOffset (rot); 49 | } 50 | 51 | 52 | void Pose::set(const Point3 &newposition, const Quaternion &neworientation) 53 | { 54 | position = newposition; orientation = neworientation; 55 | } 56 | 57 | 58 | void Pose::getAbsolute (Point3 &absPosition, Quaternion &absOrientation) 59 | { 60 | Point3 newpos = -positionOffset; 61 | tf::Transform offset = eigen2tf (orientationOffset, newpos); 62 | tf::Transform curPose = eigen2tf (orientation, position); 63 | tf::Transform newPose = offset * curPose; 64 | 65 | tf2eigen (newPose, absOrientation, absPosition); 66 | } 67 | 68 | 69 | Vector3 Pose::getDirectionVector (bool useOffset) 70 | { 71 | Point3 v1 = transformCameraToWorld (Point3 (0, 0, 0), useOffset); 72 | Point3 v2 = transformCameraToWorld (Point3 (0, 0, 1), useOffset); 73 | Vector3 v = v2 - v1; 74 | v.normalize(); 75 | return v; 76 | } 77 | 78 | 79 | Vector3 Pose::getNormalVector (bool useOffset) 80 | { 81 | Point3 v1 = transformCameraToWorld (Point3 (0, 0, 0), useOffset); 82 | Point3 v2 = transformCameraToWorld (Point3 (0, -1, 0), useOffset); 83 | Vector3 v = v2 - v1; 84 | v.normalize(); 85 | return v; 86 | } 87 | 88 | 89 | Point3 Pose::getWorldPosition() 90 | { 91 | return transformCameraToWorld (Point3 (0, 0, 0)); 92 | } 93 | 94 | 95 | void Pose::fix() 96 | { 97 | getAbsolute (position, orientation); 98 | } 99 | 100 | 101 | Point3 Pose::transformWorldToCamera (const Point3& pointInWorld, bool useOffset) 102 | { 103 | Point3 cp; Quaternion co; 104 | 105 | if (useOffset) 106 | getAbsolute (cp, co); 107 | else { 108 | cp = position; 109 | co = orientation; 110 | } 111 | 112 | tf::Transform tfa = eigen2tf (co, cp); 113 | tf::Vector3 ptCam = tfa * eigen2tf (pointInWorld); 114 | return tf2eigen (ptCam); 115 | } 116 | 117 | 118 | Point3 Pose::transformCameraToWorld (const Point3& pointInCamera, bool useOffset) 119 | { 120 | Point3 cp; Quaternion co; 121 | 122 | if (useOffset) 123 | getAbsolute (cp, co); 124 | else { 125 | cp = position; 126 | co = orientation; 127 | } 128 | 129 | tf::Transform tfa = eigen2tf (co, cp).inverse(); 130 | tf::Vector3 ptCam = tfa * eigen2tf (pointInCamera); 131 | return tf2eigen (ptCam); 132 | } 133 | 134 | 135 | double Pose::getHeading() 136 | { 137 | Vector3 dir = getDirectionVector (); 138 | dir.z() = 0; 139 | double d = acos (dir.y() / dir.squaredNorm()); 140 | if (dir.x() < 0) 141 | return 2*M_PI - d; 142 | else return d; 143 | } 144 | 145 | 146 | /* 147 | * Negative angle returned by this function means that the vehicle is pitching down, 148 | * and vice versa. 149 | */ 150 | double Pose::getElevation() 151 | { 152 | Vector3 dir = getDirectionVector (); 153 | dir.x() = 0; 154 | dir.normalize(); 155 | double d = acos (dir.y() / dir.squaredNorm()); 156 | if (dir.z() < 0) 157 | return -d; 158 | else return d; 159 | } 160 | 161 | 162 | double Pose::getBank () 163 | { 164 | Vector3 normal = getNormalVector (); 165 | normal.y() = 0; 166 | normal.normalize(); 167 | double d = acos (normal.z() / normal.squaredNorm()); 168 | if (normal.x() < 0) 169 | return -d; 170 | else return d; 171 | } 172 | -------------------------------------------------------------------------------- /vectormap_slam/Pose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Pose.h 3 | * 4 | * Created on: Jul 28, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef _POSE_H_ 9 | #define _POSE_H_ 10 | 11 | #include "Math.h" 12 | 13 | 14 | class Pose { 15 | public: 16 | Pose() : 17 | position (0, 0, 0), 18 | orientation (Quaternion::Identity()), 19 | positionOffset (0, 0, 0), 20 | orientationOffset (Quaternion::Identity()) 21 | {} 22 | 23 | void set (const Point3 &newposition, const Quaternion &neworientation); 24 | 25 | void offset (const Point3 &posOff, const Quaternion &oriOff=Quaternion::Identity()) 26 | { 27 | positionOffset = posOff; orientationOffset = oriOff; 28 | } 29 | 30 | 31 | void offset (double x, double y, double z, double roll=0, double pitch=0, double yaw=0); 32 | 33 | Vector3 getDirectionVector (bool useOffset=true); 34 | Vector3 getNormalVector (bool useOffset=true); 35 | 36 | void offsetRoll (double roll); 37 | void offsetPitch (double pitch); 38 | void offsetYaw (double yaw); 39 | 40 | // combine transformation between fixed and offset to absolute 41 | void getAbsolute (Point3 &absPosition, Quaternion &absOrientation); 42 | // apply offset to fixed 43 | void fix (); 44 | 45 | void reset() 46 | { 47 | positionOffset = Point3 (0, 0, 0); 48 | orientationOffset = Quaternion::Identity(); 49 | } 50 | 51 | void resetOrientationOffset () 52 | { 53 | orientationOffset = Quaternion::Identity(); 54 | } 55 | 56 | Point3& getPosition () { return position; } 57 | Quaternion& getOrientation () { return orientation; } 58 | Point3& getPositionOffset () { return positionOffset; } 59 | Quaternion& getOrientationOffset () { return orientationOffset; } 60 | 61 | Point3 getWorldPosition(); 62 | 63 | Point3 transformWorldToCamera (const Point3& pointInWorld, bool useOffset=true); 64 | Point3 transformCameraToWorld (const Point3& pointInCamera, bool useOffset=true); 65 | 66 | double getHeading (); 67 | double getElevation (); 68 | double getBank (); 69 | 70 | protected: 71 | // fixed 72 | Point3 position; 73 | Quaternion orientation; 74 | // offsets 75 | Point3 positionOffset; 76 | Quaternion orientationOffset; 77 | 78 | void setRotationOffset (const Quaternion &rot); 79 | }; 80 | 81 | #endif /* _POSE_H_ */ 82 | -------------------------------------------------------------------------------- /vectormap_slam/ProjectionFormula.wxm: -------------------------------------------------------------------------------- 1 | /* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ 2 | /* [ Created with wxMaxima version 13.04.2 ] */ 3 | 4 | /* [wxMaxima: comment start ] 5 | Definition of Projection Matrices 6 | [wxMaxima: comment end ] */ 7 | 8 | /* [wxMaxima: input start ] */ 9 | P : [xs,ys,zs, 1]; 10 | T: matrix ([1,0,0,-tx], [0,1,0,-ty], [0,0,1,-tz], [0,0,0,1]); 11 | Q: [qx,qy,qz,qw]; 12 | R : matrix( 13 | [1-2*qy^2-2*qz^2, 2*qx*qy-2*qw*qz, 2*qx*qz+2*qw*qy, 0], 14 | [2*qx*qy+2*qw*qz, 1-2*qx^2-2*qz^2, 2*qy*qz-2*qw*qx, 0], 15 | [2*qx*qz-2*qw*qy, 2*qy*qz+2*qw*qx, 1-2*qx^2-2*qy^2, 0], 16 | [0, 0, 0, 1] 17 | ); 18 | /* [wxMaxima: input end ] */ 19 | 20 | /* [wxMaxima: input start ] */ 21 | C: R . T. P; 22 | /* [wxMaxima: input end ] */ 23 | 24 | /* [wxMaxima: input start ] */ 25 | x: C[1][1]; 26 | y: C[2][1]; 27 | z: C[3][1]; 28 | /* [wxMaxima: input end ] */ 29 | 30 | /* [wxMaxima: input start ] */ 31 | diff (x, tx); 32 | diff (x, ty); 33 | diff (x, tz); 34 | diff (x, qx); 35 | diff (x, qy); 36 | diff (x, qz); 37 | diff (x, qw); 38 | /* [wxMaxima: input end ] */ 39 | 40 | /* [wxMaxima: input start ] */ 41 | diff (y, tx); 42 | diff (y, ty); 43 | diff (y, tz); 44 | diff (y, qx); 45 | diff (y, qy); 46 | diff (y, qz); 47 | diff (y, qw); 48 | /* [wxMaxima: input end ] */ 49 | 50 | /* [wxMaxima: input start ] */ 51 | diff (z, tx); 52 | diff (z, ty); 53 | diff (z, tz); 54 | diff (z, qx); 55 | diff (z, qy); 56 | diff (z, qz); 57 | diff (z, qw); 58 | /* [wxMaxima: input end ] */ 59 | 60 | /* Maxima can't load/batch files which end with a comment! */ 61 | "Created with wxMaxima"$ 62 | -------------------------------------------------------------------------------- /vectormap_slam/Rate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Rate.h 3 | * 4 | * Created on: Mar 19, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef RATE_H_ 9 | #define RATE_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace boost::posix_time; 17 | 18 | 19 | class Rate { 20 | public: 21 | inline Rate (int hz) 22 | { 23 | assert (hz >= 1); 24 | float microsec = 1e6 / (float)hz; 25 | sleeptime = microseconds (microsec); 26 | lastUpdate = microsec_clock::local_time(); 27 | } 28 | 29 | inline void sleep() 30 | { 31 | ptime curtime = microsec_clock::local_time(); 32 | time_duration delay = curtime - lastUpdate; 33 | if (delay < sleeptime) { 34 | time_duration realSleepTime = sleeptime - delay; 35 | usleep (realSleepTime.total_microseconds()); 36 | } 37 | lastUpdate = microsec_clock::local_time(); 38 | } 39 | 40 | private: 41 | ptime lastUpdate; 42 | time_duration sleeptime; 43 | }; 44 | 45 | #endif /* RATE_H_ */ 46 | -------------------------------------------------------------------------------- /vectormap_slam/RenderWidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RenderWidget.h 3 | * 4 | * Created on: Jul 7, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef RENDERWIDGET_H_ 9 | #define RENDERWIDGET_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "Camera.h" 17 | #include "DrawObjects.h" 18 | #include "Pose.h" 19 | 20 | 21 | using std::vector; 22 | 23 | /* 24 | * This widget should be invisible 25 | * To grab the image, copy the contents of framebuffer 26 | */ 27 | 28 | 29 | class RenderWidget: public QGLWidget { 30 | Q_OBJECT 31 | public: 32 | 33 | enum ImageFormat 34 | { 35 | RGB, 36 | RGBA, 37 | Gray 38 | }; 39 | 40 | RenderWidget (int wWidth, int wHeight, QWidget *parent); 41 | virtual ~RenderWidget(); 42 | 43 | Camera* getCamera() { return camera; } 44 | QGLShaderProgram* getShader() { return shader; } 45 | void update (); 46 | QGLFramebufferObject* getFramebuffer () { return renderBuffer; } 47 | 48 | inline void addObject (VectorMapObject* d) 49 | { drawObjects.push_back(d); } 50 | 51 | void draw (bool useOffset=false); 52 | 53 | inline QImage getImage () { return renderBuffer->toImage(); } 54 | 55 | void getImage (cv::Mat &cvmat); 56 | 57 | void cameraToShader (QGLShaderProgram *shaderPrg); 58 | 59 | static cv::Mat convert2Cv (QImage *isrc); 60 | 61 | Pose* pose () { return &mypose; } 62 | 63 | void setStencil (cv::Mat *stencilImage); 64 | 65 | protected: 66 | void initializeGL (); 67 | void paintGL (); 68 | 69 | private: 70 | Camera *camera; 71 | QGLShaderProgram *shader; 72 | QGLFramebufferObject *renderBuffer; 73 | 74 | vector drawObjects; 75 | void cameraToShader (); 76 | void render (); 77 | 78 | QSize realsize; 79 | Pose mypose; 80 | 81 | cv::Mat *stencil; 82 | }; 83 | 84 | 85 | #include 86 | 87 | #define eigen2mat4x4(eigm, mat) \ 88 | {\ 89 | float *__m__ = &mat[0][0];\ 90 | memcpy (__m__, eigm.data(), sizeof(float)*16);\ 91 | } 92 | 93 | 94 | #define eigen2mat3x3(eigm, mat) \ 95 | {\ 96 | float *__m__ = &mat[0][0];\ 97 | memcpy (__m__, eigm.data(), sizeof(float)*9);\ 98 | } 99 | 100 | 101 | 102 | class TGLWidget : public QGLWidget { 103 | Q_OBJECT 104 | public: 105 | TGLWidget (QWidget *parent); 106 | ~TGLWidget (); 107 | 108 | protected: 109 | void initializeGL (); 110 | void paintGL (); 111 | void resizeGL (); 112 | }; 113 | 114 | 115 | 116 | 117 | #endif /* RENDERWIDGET_H_ */ 118 | -------------------------------------------------------------------------------- /vectormap_slam/Tesselator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Tesselator.cpp 3 | * 4 | * Created on: Aug 9, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "Tesselator.h" 9 | 10 | #define GLDEBUG 11 | #include "debug.h" 12 | 13 | 14 | 15 | Tesselator::Tesselator() 16 | { 17 | tessel = gluNewTess (); 18 | 19 | gluTessCallback (tessel, GLU_TESS_BEGIN_DATA, (void (*)())&Tesselator::tesselBeginCb); 20 | gluTessCallback (tessel, GLU_TESS_END_DATA, (void (*)())&Tesselator::tesselEndCb); 21 | gluTessCallback (tessel, GLU_TESS_VERTEX_DATA, (void (*)())&Tesselator::tesselVertexCb); 22 | gluTessCallback (tessel, GLU_TESS_ERROR, (void (*)())&Tesselator::tesselErrorCb); 23 | gluTessCallback (tessel, GLU_TESS_COMBINE_DATA, (void (*)())&Tesselator::tesselCombineCb); 24 | } 25 | 26 | 27 | Tesselator::~Tesselator() 28 | { 29 | gluDeleteTess (tessel); 30 | } 31 | 32 | 33 | void CALLBACK Tesselator::tesselBeginCb (GLenum type, void *data) 34 | { 35 | Tesselator *current = (Tesselator*)data; 36 | current->workp.drawType = type; 37 | } 38 | 39 | 40 | void CALLBACK Tesselator::tesselEndCb (void *data) 41 | { 42 | Tesselator *current = (Tesselator*)data; 43 | } 44 | 45 | 46 | void CALLBACK Tesselator::tesselVertexCb (void *vtxdata, void *data) 47 | { 48 | Tesselator *current = (Tesselator*)data; 49 | const GLdouble *ptr = (const GLdouble*)vtxdata; 50 | Point3 vtx (ptr[0], ptr[1], ptr[2]); 51 | current->workp.pointlist.push_back(vtx); 52 | } 53 | 54 | 55 | void CALLBACK Tesselator::tesselErrorCb (GLenum errorcode) 56 | { 57 | //Tesselator *current = (Tesselator*)data; 58 | const unsigned char *err = gluErrorString (errorcode); 59 | debug ("GL Error (%d): %s", errorcode, err); 60 | throw err; 61 | } 62 | 63 | 64 | void CALLBACK Tesselator::tesselCombineCb ( 65 | GLdouble cbvtx[3], 66 | void *vertex_data[4], 67 | GLfloat weight[4], 68 | GLdouble **outData, 69 | void *user_data) 70 | { 71 | // debug ("Combined"); 72 | Point3 newvtx (cbvtx[0], cbvtx[1], cbvtx[2]); 73 | Tesselator *current = (Tesselator*)user_data; 74 | current->workp.pointlist.push_back(newvtx); 75 | 76 | GLdouble *vtx = new GLdouble[3]; 77 | vtx[0] = cbvtx[0]; 78 | vtx[1] = cbvtx[1]; 79 | vtx[2] = cbvtx[2]; 80 | *outData = vtx; 81 | } 82 | 83 | 84 | Polygon Tesselator::tesselize (Area &area, VectorMap &map) 85 | { 86 | vector areaPoints = areaToPoints (area, map); 87 | return tesselize (areaPoints); 88 | } 89 | 90 | 91 | Polygon Tesselator::tesselize (vector &pointlst) 92 | { 93 | workp.clear (); 94 | 95 | gluTessBeginPolygon (tessel, this); 96 | gluTessBeginContour (tessel); 97 | for (int i=0; i 13 | #include 14 | #include 15 | #include "Math.h" 16 | #include "vector_map.h" 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | #ifndef CALLBACK 23 | #define CALLBACK 24 | #endif 25 | 26 | 27 | struct Polygon { 28 | GLenum drawType; 29 | vector pointlist; 30 | 31 | void clear () 32 | { 33 | drawType = 0; pointlist.clear(); 34 | } 35 | }; 36 | 37 | 38 | 39 | class Tesselator { 40 | public: 41 | Tesselator(); 42 | virtual ~Tesselator(); 43 | 44 | static void CALLBACK tesselBeginCb (GLenum type, void *data); 45 | static void CALLBACK tesselEndCb (void *data); 46 | static void CALLBACK tesselVertexCb (void *vtxdata, void *data); 47 | static void CALLBACK tesselErrorCb (GLenum errorcode); 48 | static void CALLBACK tesselCombineCb (GLdouble coords[3], 49 | void *vertex_data[4], 50 | GLfloat weight[4], 51 | GLdouble **outData, 52 | void *user_data); 53 | 54 | Polygon tesselize (Area &area, VectorMap &map); 55 | Polygon tesselize (vector &pointlst); 56 | 57 | private: 58 | GLUtesselator *tessel; 59 | Polygon workp; 60 | }; 61 | 62 | #endif /* VECTORMAP_SLAM_TESSELATOR_H_ */ 63 | -------------------------------------------------------------------------------- /vectormap_slam/TrafficLights.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TrafficLights.cpp 3 | * 4 | * Created on: Apr 13, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include 9 | #include "RenderWidget.h" 10 | 11 | 12 | // convert double angle to 3D vector 13 | Vector3 vectormapToVector3 (double Hang, double Vang) 14 | { 15 | float x = sin (Hang), 16 | y = cos (Hang), 17 | z = cos (Vang); 18 | Vector3 v (x, y, z); 19 | v.normalize(); 20 | return v; 21 | } 22 | 23 | 24 | 25 | TrafficLights::TrafficLights (VectorMap *v, RenderWidget *w) : 26 | VectorMapObject (w), 27 | vmap (v) 28 | { 29 | int ipts = 0; 30 | 31 | // check if maximum limit of this loop is right 32 | for (int i=1; isignals.size()-1; i++) { 33 | Signal signal = vmap->signals[i]; 34 | int pid = vmap->vectors[signal.vid].pid; 35 | Point3 pxyz = vmap->getPoint(pid); 36 | signalCenterPoints.push_back (pxyz); 37 | 38 | TrafficLight scirc; 39 | scirc.id = signal.id; 40 | switch (signal.type) { 41 | case 1: scirc.color = SIGNAL_RED; break; 42 | case 2: scirc.color = SIGNAL_GREEN; break; 43 | case 3: scirc.color = SIGNAL_YELLOW; break; 44 | } 45 | scirc.points = signalPoint2Circle (vmap, signal); 46 | scirc.points.push_back (pxyz); 47 | for (int j=0; jvectors[signal.vid]; 54 | scirc.direction = vectormapToVector3 (v.hang, v.vang); 55 | 56 | signalObjects.push_back(scirc); 57 | } 58 | } 59 | 60 | 61 | void TrafficLights::initialize () 62 | { 63 | initBuffer (); 64 | vbuffer->bind(); 65 | vbuffer->allocate (allPts.data(), allPts.size()*sizeof(Point3)); 66 | vbuffer->release(); 67 | } 68 | 69 | 70 | void TrafficLights::draw() 71 | { 72 | vbuffer->bind (); 73 | glEnableClientState (GL_VERTEX_ARRAY); 74 | glVertexPointer (3, GL_FLOAT, 0, 0); 75 | 76 | Vector3 vdir = glwidget->pose()->getDirectionVector(); 77 | 78 | for (int i=0; i -0.5*M_PI) 83 | // continue; 84 | 85 | switch (signal.color) { 86 | case SIGNAL_RED: 87 | setColor(255, 0, 0); break; 88 | case SIGNAL_GREEN: 89 | setColor(0, 255, 0); break; 90 | case SIGNAL_YELLOW: 91 | setColor(255, 255, 0); break; 92 | } 93 | glwidget->getShader()->setUniformValue("objColor", color2qv(color)); 94 | glDrawElements (GL_LINES, signal.idx.size(), GL_UNSIGNED_INT, signal.idx.data()); 95 | // glDrawElements (GL_TRIANGLE_FAN, signal.idx.size(), GL_UNSIGNED_INT, signal.idx.data()); 96 | } 97 | 98 | glDisableClientState (GL_VERTEX_ARRAY); 99 | vbuffer->release(); 100 | } 101 | 102 | 103 | Vector3 TrafficLights::angle2UnitVector (const float Hang, const float Vang) 104 | { 105 | Vector3 unv; 106 | unv.x() = sin (Hang * M_PI/180.0); 107 | unv.y() = cos (Hang * M_PI/180.0); 108 | unv.z() = 0.0; 109 | unv.normalize(); 110 | return unv; 111 | } 112 | 113 | 114 | vector TrafficLights::signalPoint2Circle (const Point3 &c, float Hang, float Vang) 115 | { 116 | vector circle; 117 | 118 | for (double angle=-M_PI; angle TrafficLights::signalPoint2Circle (VectorMap *map, const Signal &sgn) 141 | { 142 | Vector v = map->vectors[sgn.vid]; 143 | int pid = v.pid; 144 | Point3 pc = map->getPoint(pid); 145 | return signalPoint2Circle (pc, v.hang, v.vang); 146 | } 147 | 148 | 149 | Point3 multiply3to4 (const Matrix4 &mat, const Point3 &point, float w) 150 | { 151 | Point4 pw (point.x(), point.y(), point.z(), w); 152 | Point4 r4 = mat * pw; 153 | return Point3 (r4.x(), r4.y(), r4.z()); 154 | } 155 | 156 | 157 | Point4 p3to4 (const Point3 &p3) 158 | { 159 | return Point4 (p3.x(), p3.y(), p3.z(), 1); 160 | } 161 | 162 | 163 | Point3 p4to3 (const Point4 &p4) 164 | { 165 | return Point3 (p4.x(), p4.y(), p4.z()); 166 | } 167 | 168 | 169 | -------------------------------------------------------------------------------- /vectormap_slam/TrafficLights.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TrafficLights.h 3 | * 4 | * Created on: Apr 13, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef TRAFFICLIGHTS_H_ 9 | #define TRAFFICLIGHTS_H_ 10 | 11 | #include "DrawObjects.h" 12 | #include 13 | #include 14 | 15 | #include "vector_map.h" 16 | 17 | 18 | 19 | using std::vector; 20 | 21 | 22 | const float lampRadius = 0.2; 23 | 24 | 25 | #define SIGNAL_RED 1 26 | #define SIGNAL_GREEN 2 27 | #define SIGNAL_YELLOW 3 28 | 29 | 30 | class Camera; 31 | 32 | 33 | struct TrafficLight { 34 | Point3 center; 35 | vector points; 36 | vector idx; 37 | unsigned int color; 38 | unsigned int id; 39 | Vector3 direction; 40 | }; 41 | 42 | 43 | 44 | class TrafficLights: public VectorMapObject 45 | { 46 | public: 47 | TrafficLights (VectorMap *v, RenderWidget *w); 48 | 49 | void initialize (); 50 | 51 | void draw (); 52 | 53 | private: 54 | VectorMap *vmap; 55 | vector signalCenterPoints; 56 | 57 | vector allPts; 58 | vector signalObjects; 59 | 60 | static Vector3 angle2UnitVector (const float Hang, const float Vang); 61 | static vector signalPoint2Circle (const Point3 ¢erPoint, float Hang, float Vang); 62 | static vector signalPoint2Circle (VectorMap *map, const Signal &sgn); 63 | 64 | }; 65 | 66 | #endif /* TRAFFICLIGHTS_H_ */ 67 | -------------------------------------------------------------------------------- /vectormap_slam/Transform 2D.wxm: -------------------------------------------------------------------------------- 1 | /* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ 2 | /* [ Created with wxMaxima version 13.04.2 ] */ 3 | 4 | /* [wxMaxima: input start ] */ 5 | A: [ax, ay, 1]; 6 | B: [bx, by, 1]; 7 | T: matrix( 8 | [1,0,tx], 9 | [0,1,ty], 10 | [0,0,1] 11 | ) . matrix( 12 | [cos(t), -sin(t), 0], 13 | [sin(t), cos(t), 0], 14 | [0, 0, 1] 15 | ) . matrix( 16 | [1, 0, -tx], 17 | [0, 1, -tx], 18 | [0, 0, 1] 19 | ); 20 | At: T . A; 21 | Bt: T . B; 22 | /* [wxMaxima: input end ] */ 23 | 24 | /* [wxMaxima: input start ] */ 25 | P: [px,py,1]; 26 | /* [wxMaxima: input end ] */ 27 | 28 | /* [wxMaxima: input start ] */ 29 | Q: At + (Bt-At)*((P-At) . (Bt-At)) / ((Bt-At).(Bt-At)); 30 | /* [wxMaxima: input end ] */ 31 | 32 | /* [wxMaxima: input start ] */ 33 | e(At,Bt) := (P-Q) . (P-Q); 34 | /* [wxMaxima: input end ] */ 35 | 36 | /* [wxMaxima: input start ] */ 37 | diff (e(At,Bt), tx); 38 | /* [wxMaxima: input end ] */ 39 | 40 | 41 | /* Maxima can't load/batch files which end with a comment! */ 42 | "Created with wxMaxima"$ 43 | -------------------------------------------------------------------------------- /vectormap_slam/debug.h: -------------------------------------------------------------------------------- 1 | /* 2 | * debug.h 3 | * 4 | * Created on: Dec 10, 2014 5 | * Author: jiwo 6 | */ 7 | 8 | #ifndef DEBUG_H_ 9 | #define DEBUG_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "Math.h" 16 | 17 | 18 | #ifdef GLDEBUG 19 | #include 20 | #include 21 | #endif 22 | 23 | 24 | inline void debug (const char *strfmt, ...) 25 | { 26 | va_list args; 27 | va_start (args, strfmt); 28 | vprintf (strfmt, args); 29 | va_end (args); 30 | fprintf (stdout, "\n"); 31 | } 32 | 33 | 34 | inline void debug (const Point3 &p) 35 | { 36 | printf ("(%f, %f, %f)", p.x(), p.y(), p.z()); 37 | } 38 | 39 | 40 | inline void debug (const Quaternion &q) 41 | { 42 | printf ("(x=%f, y=%f, z=%f, w=%f)", q.x(), q.y(), q.z(), q.w()); 43 | } 44 | 45 | 46 | #ifdef GLDEBUG 47 | inline void _getGlError () 48 | { 49 | GLenum ercode = glGetError (); 50 | if (ercode != GL_NO_ERROR) { 51 | const GLubyte *errstr = gluErrorString (ercode); 52 | debug ("GL Error (%d): %s", ercode, errstr); 53 | } 54 | } 55 | 56 | 57 | #define glwrap(cmd) \ 58 | cmd; \ 59 | _getGlError (); 60 | #endif 61 | 62 | 63 | typedef boost::posix_time::microsec_clock mclock; 64 | typedef boost::posix_time::time_duration timelen; 65 | using boost::posix_time::ptime; 66 | 67 | // Timing routines 68 | #define printTime(cmd) \ 69 | boost::posix_time::ptime __t1, __t2; \ 70 | __t1 = mclock::local_time (); \ 71 | cmd; \ 72 | __t2 = mclock::local_time (); 73 | 74 | 75 | 76 | #endif /* DEBUG_H_ */ 77 | -------------------------------------------------------------------------------- /vectormap_slam/imagefilter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from __future__ import division 4 | import cv2 5 | import numpy as np 6 | import rospy 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge 9 | import os 10 | from sys import argv 11 | 12 | 13 | imageTopic = "/camera1" 14 | imageSize = (1024, 768) 15 | mask = None 16 | detector = cv2.ORB(200) 17 | 18 | 19 | def DetectFeaturesFromLines (imagesrc): 20 | global mask, detector 21 | 22 | imageproc = cv2.GaussianBlur(imagesrc, (5,5), 0) 23 | # imageproc = cv2.Sobel(imageproc, -1, 1, 1, imageproc, 3, 1, 0.5) 24 | # edges = cv2.Canny(imageproc, 50, 200, apertureSize=5) 25 | 26 | imageproc = cv2.Laplacian(imageproc, -1) 27 | 28 | keypoints, descriptors = detector.detectAndCompute (imageproc, None) 29 | imageproc = cv2.cvtColor(imageproc, cv2.COLOR_GRAY2BGR) 30 | imageproc = cv2.drawKeypoints(imageproc, keypoints, flags=2, color=[0, 255, 0]) 31 | 32 | return imageproc 33 | 34 | 35 | def DetectLines (imagesrc): 36 | 37 | imageproc = cv2.GaussianBlur(imagesrc, (11,11), 0) 38 | edges = cv2.Canny(imageproc, 1, 50) 39 | 40 | lines = cv2.HoughLinesP(edges, 100, np.pi/180.0, 300, 100) 41 | imageproc = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR) 42 | print ("# of lines: {}".format(len(lines[0]))) 43 | 44 | for l in lines[0]: 45 | cv2.line (imageproc, (l[0],l[1]), (l[2],l[3]), (0,0,255), 2) 46 | pass 47 | 48 | return imageproc 49 | 50 | 51 | 52 | def imageHandler (imageMsg): 53 | global imageWindow, bridge, isSaved, mask, imageSize 54 | global detector 55 | 56 | image = bridge.imgmsg_to_cv2(imageMsg, 'mono8') 57 | image = cv2.resize(image, imageSize) 58 | 59 | # Apply mask 60 | cv2.bitwise_and(image, mask, image) 61 | imageproc = DetectLines (image) 62 | 63 | cv2.imshow("ImageProcessor", imageproc) 64 | cv2.imshow("CameraImage", image) 65 | 66 | 67 | def glHandler (imageMsg): 68 | global mask, bridge, imageSize 69 | 70 | image = bridge.imgmsg_to_cv2 (imageMsg, 'rgba8') 71 | #image = cv2.resize (image, imageSize) 72 | cv2.imshow("MapProjection", image) 73 | 74 | 75 | if (__name__ == '__main__') : 76 | 77 | cv2.startWindowThread() 78 | imageWindowProc = cv2.namedWindow("ImageProcessor", cv2.WINDOW_AUTOSIZE) 79 | imageWindowOrig = cv2.namedWindow("CameraImage", cv2.WINDOW_AUTOSIZE) 80 | imageWindowGl = cv2.namedWindow ("MapProjection", cv2.WINDOW_AUTOSIZE) 81 | bridge = CvBridge() 82 | 83 | # Read mask 84 | d = os.path.dirname (argv[0]) 85 | mask = cv2.imread(os.path.dirname(argv[0]) + "/mask.png") 86 | mask = cv2.resize(mask, imageSize) 87 | mask = cv2.cvtColor(mask, cv2.COLOR_RGB2GRAY) 88 | 89 | rospy.init_node ("ImageTest", anonymous=True) 90 | rospy.Subscriber (imageTopic + "/image_raw", Image, imageHandler) 91 | rospy.Subscriber ("/glimage", Image, glHandler) 92 | rospy.spin() 93 | pass 94 | -------------------------------------------------------------------------------- /vectormap_slam/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * Created on: Jul 18, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | #include "debug.h" 13 | #include "Math.h" 14 | #include "Rate.h" 15 | #include "tf_eigen.h" 16 | #include 17 | #include 18 | 19 | #include "sensor_msgs/Image.h" 20 | #include 21 | #include 22 | 23 | #ifdef PUBLISH_GL 24 | #include "sensor_msgs/fill_image.h" 25 | #include "sensor_msgs/image_encodings.h" 26 | #endif 27 | 28 | #include 29 | #include "MatchWindow.h" 30 | #include "RenderWidget.h" 31 | 32 | 33 | using std::string; 34 | 35 | 36 | const string cameraTopic ("camera1"); 37 | bool _doExit = false; 38 | const int defaultRate = 50; 39 | MatchWindow *mainwindow; 40 | 41 | 42 | Eigen::Quaternionf orientation; 43 | Point3 position; 44 | 45 | #ifdef PUBLISH_GL 46 | image_transport::ImageTransport *imgtrans; 47 | image_transport::Publisher imgSend; 48 | #endif 49 | 50 | 51 | void interrupt (int s) 52 | { 53 | _doExit = true; 54 | } 55 | 56 | 57 | void getTransform (Eigen::Quaternionf &ori, Point3 &pos) 58 | { 59 | static tf::TransformListener listener; 60 | tf::StampedTransform transform; 61 | 62 | // target_frame source_frame 63 | ros::Time t = ros::Time(); 64 | listener.waitForTransform (cameraTopic, "japan_7", t, ros::Duration(10.0)); 65 | listener.lookupTransform (cameraTopic, "japan_7", t, transform); 66 | 67 | tf2eigen (transform, ori, pos); 68 | } 69 | 70 | 71 | void image_data_recv (const sensor_msgs::Image::ConstPtr &imagemsg) 72 | { 73 | // XXX: make getTransform new thread 74 | try { 75 | getTransform (orientation, position); 76 | mainwindow->setPose (position, orientation); 77 | mainwindow->update(); 78 | 79 | #ifdef PUBLISH_GL 80 | cv::Mat imageSynth; 81 | mainwindow->canvas()->getImage(imageSynth); 82 | sensor_msgs::Image msg; 83 | msg.header.stamp = ros::Time::now(); 84 | sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::RGBA8, imageSynth.rows, imageSynth.cols, imageSynth.cols*4, imageSynth.data); 85 | imgSend.publish(msg); 86 | #endif 87 | 88 | } catch (tf::TransformException &exc) { 89 | debug ("getTransform() failed"); 90 | } 91 | 92 | cv_bridge::CvImageConstPtr convImage = cv_bridge::toCvShare(imagemsg, "rgb8"); 93 | //cv::cvtColor(convImage->image, convImage->image, CV_BGR2RGB); 94 | mainwindow->setImageSource (convImage->image); 95 | } 96 | 97 | 98 | int main (int argc, char **argv) 99 | { 100 | // ROS business 101 | ros::init(argc, argv, "vmslam", ros::init_options::AnonymousName); 102 | ros::NodeHandle rosnode; 103 | ros::Subscriber imgRecv = rosnode.subscribe (cameraTopic+"/image_raw", 10, image_data_recv); 104 | 105 | #ifdef PUBLISH_GL 106 | imgtrans = new image_transport::ImageTransport (rosnode); 107 | imgSend = imgtrans->advertise ("/glimage", 1); 108 | #endif 109 | 110 | // QT 111 | QApplication app (argc, argv); 112 | mainwindow = new MatchWindow (); 113 | mainwindow->show(); 114 | 115 | Rate loop (defaultRate); 116 | signal (SIGINT, interrupt); 117 | 118 | while (true) { 119 | 120 | if (mainwindow->isClosed() == true || _doExit==true) 121 | break; 122 | 123 | ros::spinOnce(); 124 | app.processEvents(); 125 | loop.sleep(); 126 | } 127 | 128 | delete (mainwindow); 129 | 130 | #ifdef PUBLISH_GL 131 | delete (imgtrans); 132 | #endif 133 | 134 | return 0; 135 | } 136 | -------------------------------------------------------------------------------- /vectormap_slam/mask.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/mask.png -------------------------------------------------------------------------------- /vectormap_slam/mask.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/mask.xcf -------------------------------------------------------------------------------- /vectormap_slam/nmi.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from __future__ import division 4 | import cv2 5 | import numpy as np 6 | import os 7 | from sys import argv 8 | import math 9 | from copy import copy 10 | 11 | 12 | 13 | def logp (val): 14 | try : 15 | return math.log(val) 16 | except : 17 | return 0 18 | 19 | 20 | def histogramBinary1 (image1): 21 | hist = np.zeros(2) 22 | for r in range (image1.shape[0]) : 23 | for c in range (image1.shape[1]) : 24 | if image1[r,c] == 0 : 25 | hist[0] += 1 26 | else : 27 | hist[1] += 1 28 | return hist 29 | 30 | 31 | def histogramBinary2 (image1, image2): 32 | hist = np.zeros ((2,2)) 33 | if (image1.shape != image2.shape) : 34 | raise Exception ("Mismatched dimension") 35 | for r in range (image1.shape[0]) : 36 | for c in range (image1.shape[1]) : 37 | if image1[r,c] == 0 and image2[r,c] == 0: 38 | hist[0,0] += 1 39 | elif image1[r,c] !=0 and image2[r,c] == 0: 40 | hist[0,1] += 1 41 | elif image1[r,c] ==0 and image2[r,c] != 0: 42 | hist[1,0] += 1 43 | else : 44 | hist[1,1] += 1 45 | return hist 46 | 47 | 48 | def imagebw (): 49 | im = np.zeros ((400,400), np.uint8) 50 | for i in range (im.shape[0]) : 51 | for j in range (im.shape[1]) : 52 | x = i/400.0 * math.pi * 8 53 | y = j/400.0 54 | im[i,j] = np.uint8(round((math.cos(x*y)+1)/2.)*255) 55 | return im 56 | 57 | 58 | def entropy1 (image1): 59 | hst = histogramBinary1 (image1) 60 | N = image1.shape[0] * image1.shape[1] 61 | return sum ([p*logp(p) for p in hst/N]) * (-1) 62 | 63 | 64 | def entropy2 (image1, image2): 65 | hst = histogramBinary2(image1, image2) 66 | N = image1.shape[0] * image1.shape[1] 67 | hst /= N 68 | plog = copy (hst) 69 | plog [0,0] = logp (plog[0,0]) 70 | plog [0,1] = logp (plog[0,1]) 71 | plog [1,0] = logp (plog[1,0]) 72 | plog [1,1] = logp (plog[1,1]) 73 | hst = hst * plog 74 | s = sum (sum(hst)) * (-1) 75 | return s 76 | 77 | 78 | def nmi (image1, image2): 79 | return (entropy1(image1) + entropy1(image2)) / entropy2(image1, image2) 80 | 81 | 82 | if __name__ == '__main__' : 83 | 84 | image1 = cv2.imread ("/var/tmp/test1.png", cv2.CV_LOAD_IMAGE_GRAYSCALE) 85 | image2 = cv2.imread ("/var/tmp/test2.png", cv2.CV_LOAD_IMAGE_GRAYSCALE) 86 | image3 = copy (image1) 87 | 88 | e1 = entropy2 (image1, image2) 89 | e2 = entropy2 (image1, image3) 90 | 91 | pass -------------------------------------------------------------------------------- /vectormap_slam/notes/flowchart_for_line_projection.txt: -------------------------------------------------------------------------------- 1 | foreach lines as lineSegment : 2 | project point A, B from lineSegment 3 | if (A and B are out of screen) OR (A and B are beyond FAR PLANE): 4 | skip 5 | compute Jacobian values 6 | store lineSegment 7 | -------------------------------------------------------------------------------- /vectormap_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vectormap_slam 4 | 0.0.0 5 | The vectormap_slam package 6 | 7 | 8 | 9 | 10 | sujiwo 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /vectormap_slam/params1.txt: -------------------------------------------------------------------------------- 1 | Position: 92200.109375 3582.854248 23418.195312 2 | Orientation: 0.613540 0.581302 0.380021 -0.375820 3 | Camera: 1150.969360 1150.969360 1920.000000 1440.000000 988.511353 692.803955 4 | -------------------------------------------------------------------------------- /vectormap_slam/plotheatmap.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import math 5 | from copy import copy 6 | from mpl_toolkits.mplot3d import Axes3D 7 | 8 | 9 | GridSize = 51 10 | MaxOffset = 2.0 11 | 12 | 13 | if (__name__ == '__main__') : 14 | 15 | fig = plt.figure() 16 | ax = fig.add_subplot (111, projection='3d') 17 | 18 | step = MaxOffset / math.floor(GridSize/2) 19 | X = np.arange(-MaxOffset, MaxOffset+step, step) 20 | Y = copy(X) 21 | Z = np.loadtxt ("/tmp/heatmap.csv", delimiter=",") 22 | Xm, Ym = np.meshgrid(X, Y) 23 | 24 | ax.plot_surface (Xm, Ym, Z) 25 | 26 | plt.show() 27 | pass -------------------------------------------------------------------------------- /vectormap_slam/projectpoint.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import cv2 3 | import numpy as np 4 | import math 5 | 6 | 7 | points = np.array([[0, 0, -2], 8 | [1, 0, -2], 9 | [1, 1, -2], 10 | [0, 1, -2]], dtype=np.float) 11 | 12 | width = 800 13 | height = 600 14 | 15 | 16 | def degreeToRadian (degree) : 17 | return degree * np.pi / 180.0; 18 | 19 | 20 | def lookAt (eyePos, centerOfView, upVect): 21 | viewMat = np.eye (4) 22 | direction = centerOfView - eyePos 23 | direction = direction / np.linalg.norm (direction) 24 | 25 | upVect = upVect / np.linalg.norm (upVect) 26 | side = np.cross (direction, upVect) 27 | Utrue = np.cross (side, direction) 28 | 29 | viewMat [0, 0:3] = side 30 | viewMat [1, 0:3] = Utrue 31 | viewMat [2, 0:3] = -direction 32 | viewMat [0:3, 3] = -eyePos 33 | 34 | return viewMat 35 | 36 | def perspective1 (fovy, aspectRatio, zNear, zFar): 37 | projMat = np.eye (4) 38 | angle = degreeToRadian(fovy/2.0) 39 | f = 1 / math.tan (angle) 40 | projMat [0, 0] = f / aspectRatio 41 | projMat [1, 1] = f 42 | projMat [2, 2] = (zFar + zNear) / (zNear - zFar) 43 | projMat [3, 2] = -1 44 | projMat [2, 3] = 2 * zFar * zNear / (zNear - zFar) 45 | return projMat 46 | 47 | # def perspective2 (f, ): 48 | 49 | 50 | def plotPoints (image, pointList): 51 | origX = image.shape[1] / 2 52 | origY = image.shape[0] / 2 53 | for point in pointList: 54 | point = point[0] 55 | u = origY * (1 + point[0]) 56 | v = origX * (1 + point[1]) 57 | u = int(u) 58 | v = int(v) 59 | image [u, v] = 255 60 | image [u+1, v+1] = 255 61 | image [u, v+1] = 255 62 | image [u+1, v] = 255 63 | pass 64 | 65 | 66 | 67 | if __name__ == '__main__' : 68 | viewMat = lookAt (np.array([-0.5, -0.5, 2]), 69 | np.array([0.5, 0.5, -2]), 70 | np.array([0, 1, 0])) 71 | # projMat = perspective1 (45.0, 1.33, 1.0, 100) 72 | projMat = np.eye(3) 73 | 74 | image = np.zeros ((height,width), dtype=np.uint8) 75 | # rv, jac1 = cv2.Rodrigues(viewMat[0:3, 0:3]) 76 | rv, jac1 = cv2.Rodrigues(np.eye(3)) 77 | # imgpts, jac2 = cv2.projectPoints(points, rv, viewMat[0:3, 3], projMat[0:3,0:3], 0) 78 | imgpts, jac2 = cv2.projectPoints (points, rv, (0,0,0), np.eye(3), 0) 79 | plotPoints (image, imgpts) 80 | 81 | cv2.startWindowThread() 82 | cv2.namedWindow("test") 83 | cv2.imshow("test", image) 84 | 85 | pass -------------------------------------------------------------------------------- /vectormap_slam/reference1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/reference1.png -------------------------------------------------------------------------------- /vectormap_slam/reference2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/reference2.png -------------------------------------------------------------------------------- /vectormap_slam/reference3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/reference3.png -------------------------------------------------------------------------------- /vectormap_slam/solvertest.cpp: -------------------------------------------------------------------------------- 1 | #include "PointSolver2.h" 2 | #include 3 | #include 4 | #include 5 | #include "debug.h" 6 | #include "Math.h" 7 | 8 | 9 | using std::vector; 10 | 11 | vector model; 12 | Point3 rectangle[] = { 13 | Point3 (0, 0, -2), 14 | Point3 (1, 0, -2), 15 | Point3 (1, 1, -2), 16 | Point3 (0, 1, -2) 17 | }; 18 | 19 | 20 | void buildModel (vector &dataset, Point3 *points, int numOfPt) 21 | { 22 | dataset.clear(); 23 | for (int i=0; i &dataset, vector &pointList) 33 | { 34 | return buildModel (dataset, pointList.data(), pointList.size()); 35 | } 36 | 37 | 38 | int main (int argc, char **argv) 39 | { 40 | buildModel (model, rectangle, 4); 41 | 42 | // Point3 eyePos (-0.915031, -0.943627, 1.656656); 43 | // Quaternion eyeDir (0.985495, -0.117826, 0.121295, -0.014295); 44 | Point3 eyePos (0.5, 0.5, 2), 45 | centerOfView (0.5, 0.5, -2); 46 | Vector3 up (0, 1, 0); 47 | PointSolver2::Projector projector (45.0, 640, 480); 48 | 49 | // For testing projection 50 | // cv::Mat image; 51 | // PointSolver2::projectModel (image, model, projector, eyePos, centerOfView, up); 52 | // cv::imwrite ("/tmp/norm.png", image); 53 | 54 | cv::Mat timage = cv::imread ("/tmp/test.png", CV_LOAD_IMAGE_GRAYSCALE); 55 | 56 | PointSolver2 solver (model, projector); 57 | 58 | // initial pose 59 | Matrix4 vm = createViewMatrix (eyePos, centerOfView, up); 60 | Quaternion eyeDir; 61 | poseFromViewMatrix (vm, eyePos, eyeDir); 62 | solver.solve (timage, eyePos, eyeDir); 63 | } 64 | -------------------------------------------------------------------------------- /vectormap_slam/test_cases/test1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sujiwo/vector-map-localization/5b2bd4970bed5042a43d060f4af389fe178729b6/vectormap_slam/test_cases/test1.png -------------------------------------------------------------------------------- /vectormap_slam/test_hist.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_hist.cpp 3 | * 4 | * Created on: Aug 12, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "debug.h" 9 | #include 10 | #include 11 | #include "ImageMatch.h" 12 | 13 | 14 | 15 | void histogramBinary (cv::Mat &image, double hist[2]) 16 | { 17 | hist[0] = hist[1] = 0; 18 | 19 | for (int r=0; r(c, r)==0) 22 | hist[0] += 1; 23 | else hist[1] += 1; 24 | } 25 | } 26 | 27 | return; 28 | } 29 | 30 | 31 | void histogramBinary (cv::Mat &imageGray1, cv::Mat &imageGray2, cv::Mat &hist) 32 | { 33 | hist = cv::Mat::zeros(2, 2, CV_32F); 34 | 35 | for (int r=0; r(r,c)==0 and imageGray2.at(r,c)==0) 38 | hist.at(0, 0) += 1; 39 | else if (imageGray1.at(r,c)!=0 and imageGray2.at(r,c)==0) 40 | hist.at(0, 1) += 1; 41 | else if (imageGray1.at(r,c)==0 and imageGray2.at(r,c)!=0) 42 | hist.at(1, 0) += 1; 43 | else if (imageGray1.at(r,c)!=0 and imageGray2.at(r,c)!=0) 44 | hist.at(1, 1) += 1; 45 | } 46 | } 47 | } 48 | 49 | 50 | int main (int argc, char *argv[]) 51 | { 52 | cv::Mat img1 = cv::imread ("/var/tmp/test3.png", CV_LOAD_IMAGE_GRAYSCALE); 53 | cv::Mat img2 = cv::imread ("/var/tmp/test4.png", CV_LOAD_IMAGE_GRAYSCALE); 54 | 55 | double entropy = ImageMatch::entropy (img1, img2); 56 | 57 | debug ("%f", entropy); 58 | } 59 | -------------------------------------------------------------------------------- /vectormap_slam/test_tesselator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_tesselator.cpp 3 | * 4 | * Created on: Aug 10, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #include "Tesselator.h" 9 | #include 10 | #include "vector_map.h" 11 | #include "debug.h" 12 | #include 13 | #include 14 | 15 | 16 | using std::vector; 17 | using std::cout; 18 | using std::endl; 19 | 20 | 21 | #ifndef CALLBACK 22 | #define CALLBACK 23 | #endif 24 | 25 | 26 | void CALLBACK tessBeginCb (GLenum which) 27 | { 28 | cout << "Type: " << which << endl; 29 | } 30 | 31 | 32 | void CALLBACK tessVertexCb (const GLvoid *data) 33 | { 34 | Point3 *p = (Point3*)data; 35 | cout << p << endl; 36 | } 37 | 38 | 39 | void CALLBACK tessEndCb () 40 | { 41 | cout << "End" << endl; 42 | } 43 | 44 | 45 | const char* getPrimitiveType(GLenum type) 46 | { 47 | switch(type) 48 | { 49 | case 0x0000: 50 | return "GL_POINTS"; 51 | break; 52 | case 0x0001: 53 | return "GL_LINES"; 54 | break; 55 | case 0x0002: 56 | return "GL_LINE_LOOP"; 57 | break; 58 | case 0x0003: 59 | return "GL_LINE_STRIP"; 60 | break; 61 | case 0x0004: 62 | return "GL_TRIANGLES"; 63 | break; 64 | case 0x0005: 65 | return "GL_TRIANGLE_STRIP"; 66 | break; 67 | case 0x0006: 68 | return "GL_TRIANGLE_FAN"; 69 | break; 70 | case 0x0007: 71 | return "GL_QUADS"; 72 | break; 73 | case 0x0008: 74 | return "GL_QUAD_STRIP"; 75 | break; 76 | case 0x0009: 77 | return "GL_POLYGON"; 78 | break; 79 | } 80 | } 81 | 82 | 83 | int main (int argc, char *argv[]) 84 | { 85 | VectorMap vmmap; 86 | vmmap.loadAll("/var/tmp/nuvm"); 87 | 88 | Area a1 = vmmap.areas[1]; 89 | // vector quad; 90 | // quad.push_back (Point3 (-1, 3, 0)); 91 | // quad.push_back (Point3 (0, 0, 0)); 92 | // quad.push_back (Point3 (1, 3, 0)); 93 | // quad.push_back (Point3 (0, 2, 0)); 94 | 95 | Tesselator tesselator; 96 | Polygon plg = tesselator.tesselize (a1, vmmap); 97 | 98 | debug ("Type: %s", getPrimitiveType (plg.drawType)); 99 | 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /vectormap_slam/tf_eigen.h: -------------------------------------------------------------------------------- 1 | /* 2 | * tf_eigen.h 3 | * 4 | * Created on: Jul 18, 2015 5 | * Author: sujiwo 6 | */ 7 | 8 | #ifndef TF_EIGEN_H_ 9 | #define TF_EIGEN_H_ 10 | 11 | 12 | #include 13 | #include "Math.h" 14 | 15 | 16 | inline void tf2eigen (const tf::Transform &transform, Eigen::Quaternionf &orientation, Point3 &position) 17 | { 18 | position.x()=transform.getOrigin().x(); 19 | position.y()=transform.getOrigin().y(); 20 | position.z()=transform.getOrigin().z(); 21 | orientation.w()=transform.getRotation().w(); 22 | orientation.x()=transform.getRotation().x(); 23 | orientation.y()=transform.getRotation().y(); 24 | orientation.z()=transform.getRotation().z(); 25 | } 26 | 27 | 28 | inline tf::Transform eigen2tf (const Eigen::Quaternionf &orientation, const Point3 &position) 29 | { 30 | tf::Transform trf; 31 | tf::Vector3 p (position.x(), position.y(), position.z()); 32 | tf::Quaternion o (orientation.x(), orientation.y(), orientation.z(), orientation.w()); 33 | 34 | trf.setOrigin(p); 35 | trf.setRotation(o); 36 | return trf; 37 | } 38 | 39 | 40 | inline tf::Vector3 eigen2tf (const Vector3 &v) 41 | { 42 | return tf::Vector3 (v.x(), v.y(), v.z()); 43 | } 44 | 45 | 46 | inline Vector3 tf2eigen (const tf::Vector3 &v) 47 | { 48 | return Vector3 (v.x(), v.y(), v.z()); 49 | } 50 | 51 | 52 | inline void getRPY (const Eigen::Quaternionf &rot, double &roll, double &pitch, double &yaw) 53 | { 54 | // Test Quaternion <==> RPY 55 | tf::Quaternion o (rot.x(), rot.y(), rot.z(), rot.w()); 56 | tf::Matrix3x3 mat3x3 (o); 57 | mat3x3.getRPY (roll, pitch, yaw); 58 | } 59 | 60 | 61 | inline Eigen::Quaternionf getQuaternion (const double &roll, const double &pitch, double &yaw) 62 | { 63 | tf::Matrix3x3 mat33; 64 | mat33.setRPY(roll, pitch, yaw); 65 | tf::Quaternion q; 66 | mat33.getRotation(q); 67 | 68 | return Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z()); 69 | } 70 | 71 | #endif /* TF_EIGEN_H_ */ 72 | -------------------------------------------------------------------------------- /vectormap_slam/vector_map.h: -------------------------------------------------------------------------------- 1 | #ifndef __VECTOR_MAP__ 2 | #define __VECTOR_MAP__ 3 | 4 | //#include "ros/ros.h" 5 | //#include "std_msgs/String.h" 6 | #define QT_NO_KEYWORDS 7 | #undef signals 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "Math.h" 16 | 17 | 18 | using std::vector; 19 | using std::string; 20 | 21 | 22 | typedef struct{ 23 | int pid; 24 | double bx; 25 | double ly; 26 | double h; 27 | double b; 28 | double l; 29 | int ref; 30 | int mcode1; 31 | int mcode2; 32 | int mcode3; 33 | }Point; 34 | 35 | typedef struct{ 36 | int lid; 37 | int bpid; 38 | int fpid; 39 | int blid; 40 | int flid; 41 | }Line; 42 | 43 | typedef struct{ 44 | int vid; 45 | int pid; 46 | double hang; 47 | double vang; 48 | }Vector; 49 | 50 | 51 | typedef struct{ 52 | int id; 53 | int vid; 54 | int plid; 55 | int type; 56 | int linkid; 57 | }Signal; 58 | 59 | typedef struct{ 60 | int id; 61 | int lid; 62 | double width; 63 | char color; 64 | int type; 65 | int linkid; 66 | }CLine; 67 | 68 | typedef struct{ 69 | int lid; 70 | }Mark; 71 | 72 | typedef struct{ 73 | int did; 74 | double dist; 75 | int pid; 76 | double dir; 77 | double apara; 78 | double r; 79 | double slope; 80 | double cant; 81 | double lw; 82 | double rw; 83 | }DTLane; 84 | 85 | typedef struct{ 86 | int lnid; 87 | int did; 88 | int blid; 89 | int flid; 90 | int bnid; 91 | int fnid; 92 | int jct; 93 | int blid2; 94 | int blid3; 95 | int blid4; 96 | int flid2; 97 | int flid3; 98 | int flid4; 99 | int clossid; 100 | double span; 101 | int lcnt; 102 | int lno; 103 | }Lane; 104 | 105 | 106 | struct Pole { 107 | int id, vid; 108 | double length, dim; 109 | }; 110 | 111 | 112 | struct Area { 113 | int aid; 114 | int slid, elid; 115 | }; 116 | 117 | 118 | class VectorMap 119 | { 120 | public: 121 | bool loaded; 122 | std::map points; 123 | std::map lines; 124 | std::map clines; 125 | std::map lanes; 126 | std::map dtlanes; 127 | std::map vectors; 128 | std::map signals; 129 | std::map areas; 130 | std::map poles; 131 | 132 | int load_points(char *name); 133 | int load_lines(char *name); 134 | int load_lanes(char *name); 135 | int load_vectors(char *name); 136 | int load_signals(char *name); 137 | int load_clines(char *name); 138 | int load_dtlanes(char *name ); 139 | int load_areas (const char *name); 140 | void load_poles (const std::string &polefilename); 141 | 142 | 143 | VectorMap () : 144 | loaded(false) {} 145 | 146 | 147 | void loadAll (const std::string &dir); 148 | 149 | inline Point3 getPoint (const int idx) 150 | { 151 | Point3 p; 152 | Point psrc = points[idx]; 153 | p.x() = psrc.bx; p.y() = psrc.ly, p.z() = psrc.h; 154 | return p; 155 | } 156 | 157 | string dirname; 158 | }; 159 | 160 | 161 | vector areaToPoints (Area &a, VectorMap &map); 162 | 163 | #endif 164 | -------------------------------------------------------------------------------- /vislab_3dv_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vislab_3dv_node) 3 | 4 | find_package (catkin REQUIRED COMPONENTS 5 | roscpp 6 | image_transport 7 | pcl_ros 8 | pcl_conversions 9 | ) 10 | 11 | find_package (Boost REQUIRED COMPONENTS 12 | system 13 | thread 14 | chrono 15 | ) 16 | 17 | find_package (PCL REQUIRED COMPONENTS 18 | common 19 | io 20 | ) 21 | 22 | set (CMAKE_BUILD_TYPE Debug) 23 | set_property (GLOBAL 24 | PROPERTY COMPILE_DEFINITIONS DEBUG=1 25 | ) 26 | 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES vislab_3dv_node 31 | # CATKIN_DEPENDS other_catkin_pkg 32 | # DEPENDS system_lib 33 | ) 34 | 35 | 36 | include_directories (${PCL_INCLUDE_DIRS}) 37 | link_directories (${PCL_LIBRARY_DIRS}) 38 | add_definitions (${PCL_DEFINITIONS}) 39 | 40 | 41 | add_executable(vislab_3dv_node vislab_3dv_node.cpp) 42 | target_link_libraries(vislab_3dv_node 43 | ${catkin_LIBRARIES} 44 | 3dv 45 | dsi 46 | ${Boost_SYSTEM_LIBRARY} 47 | ${Boost_CHRONO_LIBRARY} 48 | ${PCL_COMMON_LIBRARY} 49 | ) 50 | 51 | 52 | install ( 53 | TARGETS vislab_3dv_node 54 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | ) 56 | -------------------------------------------------------------------------------- /vislab_3dv_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vislab_3dv_node 4 | 0.0.0 5 | The vislab_3dv_node package 6 | 7 | 8 | 9 | 10 | sujiwo 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | --------------------------------------------------------------------------------