├── msg ├── FeatureMatch.msg ├── RobotPose.msg ├── VisionFactor.msg ├── VisionFeature.msg ├── SLAMNode.msg ├── SLAMProblem.msg ├── OdometryFactor.msg ├── CameraExtrinsics.msg └── CameraIntrinsics.msg ├── img └── Finished.png ├── install_ubuntu.sh ├── .gitmodules ├── jenkins-ci-build.sh ├── .gitignore ├── profile ├── manifest.xml ├── Makefile ├── CMakeLists.txt ├── src ├── gui_helpers.cc ├── test │ └── bag_extract.cc ├── gui_helpers.h ├── slam_to_ros.h ├── math_util.h ├── slam_frontend.h ├── slam_types.h ├── slam_frontend_main.cc └── slam_frontend.cc ├── README.md └── visualization.rviz /msg/FeatureMatch.msg: -------------------------------------------------------------------------------- 1 | uint64 id_initial 2 | uint64 id_current 3 | -------------------------------------------------------------------------------- /msg/RobotPose.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 loc 2 | geometry_msgs/Quaternion angle -------------------------------------------------------------------------------- /img/Finished.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ut-amrl/vision_slam_frontend/HEAD/img/Finished.png -------------------------------------------------------------------------------- /msg/VisionFactor.msg: -------------------------------------------------------------------------------- 1 | uint64 pose_initial 2 | uint64 pose_current 3 | FeatureMatch[] feature_matches -------------------------------------------------------------------------------- /msg/VisionFeature.msg: -------------------------------------------------------------------------------- 1 | uint64 id 2 | geometry_msgs/Point pixel 3 | geometry_msgs/Point point3d 4 | -------------------------------------------------------------------------------- /msg/SLAMNode.msg: -------------------------------------------------------------------------------- 1 | uint64 id 2 | float64 timestamp 3 | RobotPose pose 4 | VisionFeature[] features 5 | -------------------------------------------------------------------------------- /msg/SLAMProblem.msg: -------------------------------------------------------------------------------- 1 | SLAMNode[] nodes 2 | VisionFactor[] vision_factors 3 | OdometryFactor[] odometry_factors -------------------------------------------------------------------------------- /install_ubuntu.sh: -------------------------------------------------------------------------------- 1 | # Installs dependencies for Ubuntu. 2 | 3 | sudo apt install libgflags-dev libgoogle-glog-dev 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/shared"] 2 | path = src/shared 3 | url = git@github.com:umass-amrl/amrl_shared_lib.git 4 | -------------------------------------------------------------------------------- /msg/OdometryFactor.msg: -------------------------------------------------------------------------------- 1 | uint64 pose_i 2 | uint64 pose_j 3 | geometry_msgs/Vector3 translation 4 | geometry_msgs/Quaternion rotation -------------------------------------------------------------------------------- /msg/CameraExtrinsics.msg: -------------------------------------------------------------------------------- 1 | # 3D vector of translation. 2 | float64[3] translation 3 | # Rotation in scaled angle-axis (Lie algebra) form. 4 | float64[3] rotation 5 | -------------------------------------------------------------------------------- /msg/CameraIntrinsics.msg: -------------------------------------------------------------------------------- 1 | # Focal length x. 2 | float64 fx 3 | # Focal length y. 4 | float64 fy 5 | # Principal point x. 6 | float64 cx 7 | # Principal point y. 8 | float64 cy 9 | -------------------------------------------------------------------------------- /jenkins-ci-build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /opt/ros/melodic/setup.bash 3 | export ROS_PACKAGE_PATH=`pwd`:$ROS_PACKAGE_PATH 4 | rm -rf build lib bin msg_gen 5 | mkdir build 6 | make -j8 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | build_debug 3 | bin 4 | lib 5 | *.kdev4 6 | msg_gen 7 | src/laser_extractor 8 | data 9 | core 10 | src/vision_slam_frontend 11 | *.bag 12 | profile_results 13 | 14 | -------------------------------------------------------------------------------- /profile: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # (C) Joydeep Biswas, 2010, 2017 3 | # A simple Bash script to profile a command and 4 | # display the results on termination. 5 | # You'll need to install the following packages: valgrind, kcachegrind 6 | if [ ! -d "profile_results" ]; then 7 | mkdir profile_results 8 | fi 9 | echo Profiling \"$@\" 10 | valgrind --tool=callgrind --dump-instr=yes --trace-jump=yes --callgrind-out-file="profile_results/callgrind.out.%p" $@ 11 | kcachegrind `ls -t1 profile_results/callgrind.out.*|head -n 1` 12 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Vision SLAM Frontend 5 | 6 | 7 | slam_frontend 8 | Joydeep Biswas 9 | MIT 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Clang is a good compiler to use during development due to its faster compile 2 | # times and more readable output. 3 | # C_compiler=/usr/bin/clang 4 | # CXX_compiler=/usr/bin/clang++ 5 | 6 | # GCC is better for release mode due to the speed of its output, and its support 7 | # for OpenMP. 8 | C_compiler=/usr/bin/gcc 9 | CXX_compiler=/usr/bin/g++ 10 | 11 | # acceptable build_types: Release/Debug/Profile 12 | build_type=Release 13 | # build_type=Debug 14 | 15 | all: build/CMakeLists.txt.copy 16 | $(info Build_type is [${build_type}]) 17 | $(MAKE) --no-print-directory -C build 18 | 19 | clean: 20 | rm -rf bin lib build msg_gen 21 | 22 | build/CMakeLists.txt.copy: build CMakeLists.txt Makefile 23 | cd build && cmake -DCMAKE_BUILD_TYPE=$(build_type) \ 24 | -DCMAKE_CXX_COMPILER=$(CXX_compiler) \ 25 | -DCMAKE_C_COMPILER=$(C_compiler) .. 26 | cp CMakeLists.txt build/CMakeLists.txt.copy 27 | 28 | build: 29 | mkdir -p build 30 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(vision_slam_frontend) 2 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 3 | 4 | OPTION(USE_XFEATURES "Compile with OpenCV extra features module" OFF) 5 | 6 | MESSAGE(STATUS "Compiling Slam Frontend") 7 | MESSAGE(STATUS "Using compiler: ${CMAKE_CXX_COMPILER}") 8 | MESSAGE(STATUS "Build Type: ${CMAKE_BUILD_TYPE}") 9 | MESSAGE(STATUS "Arch: ${CMAKE_SYSTEM_PROCESSOR}") 10 | 11 | SET(CMAKE_CXX_FLAGS "-std=c++11 -Wall -Werror") 12 | 13 | IF(${CMAKE_BUILD_TYPE} MATCHES "Release") 14 | MESSAGE(STATUS "Additional Flags for Release mode") 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp -O3 -DNDEBUG") 16 | ELSEIF(${CMAKE_BUILD_TYPE} MATCHES "Debug") 17 | MESSAGE(STATUS "Additional Flags for Debug mode") 18 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 19 | ENDIF() 20 | 21 | FIND_PACKAGE(OpenCV EXACT 3.2.0 REQUIRED) 22 | INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) 23 | INCLUDE($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 24 | 25 | rosbuild_init() 26 | 27 | SET(ROS_BUILD_STATIC_LIBS true) 28 | SET(ROS_BUILD_SHARED_LIBS false) 29 | 30 | MESSAGE(STATUS "ROS-Overrride Build Type: ${CMAKE_BUILD_TYPE}") 31 | MESSAGE(STATUS "CXX Flags: ${CMAKE_CXX_FLAGS}") 32 | 33 | SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 34 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 35 | 36 | # Ask ROS to compile custom message type definitions. 37 | rosbuild_genmsg() 38 | 39 | INCLUDE_DIRECTORIES(src/shared) 40 | ADD_SUBDIRECTORY(src/shared) 41 | LINK_DIRECTORIES(src/shared/build) 42 | 43 | SET(libs roslib roscpp rosbag console_bridge cv_bridge glog gflags boost_system) 44 | 45 | IF (USE_XFEATURES) 46 | MESSAGE(STATUS "Compiling with OpenCV xfeatures") 47 | ADD_DEFINITIONS(-DUSE_XFEATURES) 48 | SET(libs ${libs} opencv_xfeatures2d) 49 | ENDIF () 50 | 51 | ROSBUILD_ADD_EXECUTABLE(slam_frontend 52 | src/slam_frontend_main.cc 53 | src/slam_frontend.cc 54 | src/gui_helpers.cc) 55 | 56 | TARGET_LINK_LIBRARIES(slam_frontend 57 | ${AMRL_LIBRARY_NAME} 58 | ${OpenCV_LIBS} 59 | ${libs}) 60 | 61 | -------------------------------------------------------------------------------- /src/gui_helpers.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 joydeepb@cs.umass.edu 2 | // College of Information and Computer Sciences, 3 | // University of Massachusetts Amherst 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "gui_helpers.h" 24 | 25 | #include "glog/logging.h" 26 | #include "ros/ros.h" 27 | #include "visualization_msgs/Marker.h" 28 | #include "visualization_msgs/MarkerArray.h" 29 | 30 | namespace { 31 | int marker_id_ = 0; 32 | } // namespace 33 | namespace gui_helpers { 34 | 35 | const Color4f Color4f::kRed(1, 0, 0, 1); 36 | const Color4f Color4f::kGreen(0, 1, 0, 1); 37 | const Color4f Color4f::kBlue(0, 0, 1, 1); 38 | const Color4f Color4f::kWhite(1, 1, 1, 1); 39 | const Color4f Color4f::kBlack(0, 0, 0, 1); 40 | const Color4f Color4f::kYellow(1, 1, 0, 1); 41 | const Color4f Color4f::kCyan(0, 1, 1, 1); 42 | const Color4f Color4f::kMagenta(1, 0, 1, 1); 43 | 44 | void InitializeMarker( 45 | int marker_type, 46 | const Color4f& color, 47 | float scale_x, 48 | float scale_y, 49 | float scale_z, 50 | visualization_msgs::Marker* msg) { 51 | msg->id = marker_id_; 52 | ++marker_id_; 53 | msg->type = marker_type; 54 | msg->action = visualization_msgs::Marker::ADD; 55 | msg->pose.position.x = 0; 56 | msg->pose.position.y = 0; 57 | msg->pose.position.z = 0; 58 | msg->pose.orientation.x = 0.0; 59 | msg->pose.orientation.y = 0.0; 60 | msg->pose.orientation.z = 0.0; 61 | msg->pose.orientation.w = 1.0; 62 | msg->scale.x = scale_x; 63 | msg->scale.y = scale_y; 64 | msg->scale.z = scale_z; 65 | msg->header.frame_id = "map"; 66 | } 67 | 68 | } // namespace gui_helpers 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/test/bag_extract.cc: -------------------------------------------------------------------------------- 1 | //======================================================================== 2 | // This software is free: you can redistribute it and/or modify 3 | // it under the terms of the GNU Lesser General Public License Version 3, 4 | // as published by the Free Software Foundation. 5 | // 6 | // This software is distributed in the hope that it will be useful, 7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | // GNU Lesser General Public License for more details. 10 | // 11 | // You should have received a copy of the GNU Lesser General Public License 12 | // Version 3 in the file COPYING that came with this distribution. 13 | // If not, see . 14 | //======================================================================== 15 | /*! 16 | \file slam_frontend.cc 17 | \brief A Rosbag image extractor 18 | \author John Bachman, (C) 2019 19 | */ 20 | //======================================================================== 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include "rosbag/bag.h" 28 | #include "rosbag/view.h" 29 | #include "opencv2/opencv.hpp" 30 | #include "gflags/gflags.h" 31 | #include "glog/logging.h" 32 | #include "sensor_msgs/CompressedImage.h" 33 | 34 | DEFINE_string(input, "data/outdoor.bag", "Name of the ROS bag to have images extracted"); 35 | DEFINE_string(image_topic, "/stereo/left/image_raw/compressed", "Name of the ROS image topic to extract"); 36 | DEFINE_string(output, "data/raw_images", "Output directory for images"); 37 | 38 | int main(int argc, char** argv) { 39 | google::ParseCommandLineFlags(&argc, &argv, false); 40 | if (FLAGS_input == "") { 41 | fprintf(stderr, "ERROR: Must specify input file!\n"); 42 | return 1; 43 | } 44 | 45 | google::InitGoogleLogging(*argv); 46 | 47 | // Initialize ROS. 48 | ros::init(argc, argv, "slam_frontend"); 49 | ros::NodeHandle n; 50 | 51 | //Open bagDEFINE_output 52 | rosbag::Bag bag; 53 | try { 54 | bag.open(FLAGS_input, rosbag::bagmode::Read); 55 | } catch(rosbag::BagException exception) { 56 | LOG(ERROR) << "Unable to open " << FLAGS_input; 57 | exit(1); 58 | } 59 | 60 | std::vector image_topic; 61 | image_topic.push_back(FLAGS_image_topic.c_str()); 62 | 63 | uint64_t image_num = 0; 64 | rosbag::View view(bag, rosbag::TopicQuery(image_topic)); 65 | std::cout << "Beginning bag image extraction on topic: " 66 | << FLAGS_image_topic << std::endl; 67 | for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it) { 68 | const rosbag::MessageInstance& message = *it; 69 | ros::spinOnce(); 70 | sensor_msgs::CompressedImagePtr image_msg = 71 | message.instantiate(); 72 | if (image_msg != NULL) { 73 | cv::Mat image = cv::imdecode(cv::InputArray(image_msg->data), 1); 74 | if (image_msg->format.find("bayer_rggb8") != std::string::npos) { 75 | cv::Mat1b *image_channels = new cv::Mat1b[image.channels()]; 76 | cv::split(image, image_channels); 77 | image = image_channels[0]; 78 | cv::cvtColor(image_channels[0], image, cv::COLOR_BayerBG2BGR); 79 | delete [] image_channels; 80 | } 81 | std::stringstream ss; 82 | ss << FLAGS_output << "/image" 83 | << std::setfill('0') 84 | << std::setw(log(std::distance(view.begin(), 85 | view.end()))) 86 | << image_num++ 87 | << ".jpg"; 88 | std::string path = ss.str(); 89 | std::cout << "Writing " << path << std::endl; 90 | cv::imwrite(path, image); 91 | } 92 | } 93 | 94 | return 0; 95 | } 96 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Vision SLAM Frontend 2 | 3 | SLAM is the process of mapping your surroundings and finding out where you are at the same time (hence simultaneous-localization-and-mapping, SLAM). This project is the frontend to a SLAM system, it takes [bags](http://wiki.ros.org/Bags) from robots with stereo cameras (two cameras, left and right) and detects feature points. These feature points are then matched between camera images using their descriptors (a mathematical represenation of what makes that point special) and we extrapolate an estimate of the points position in 3D space. For now this is outputted for visualization purposes. But this can easily (and has been, but closed source for now) be integrated into a backend for optimization and loop closure for a full SLAM system. 4 | 5 | ## Dependencies 6 | 7 | You should install [ROS](http://wiki.ros.org/ROS/Installation) if you have not already. And then run the dependency install script: 8 | ``` 9 | sudo ./install_ubuntu.sh 10 | ``` 11 | 12 | Now you have to install OpenCV 3.2.0, which can be a pain so I have a archive with a working version and install instructions here: 13 | [https://github.com/TheCynosure/opencv_3.2.0_archive](https://github.com/TheCynosure/opencv_3.2.0_archive) 14 | 15 | Once that is installed you can navigate back to the `vision_slam_frontend` repo and build using the make command: 16 | 17 | ``` 18 | cd vision_slam_frontend 19 | export ROS_PACKAGE_PATH=$(pwd):$ROS_PACKAGE_PATH 20 | make 21 | ``` 22 | 23 | Tip: If you are building more than once then add the export line to your .bashrc file in your home directory to ease the building process. 24 | 25 | ### Example 26 | 27 | An example bag file is provided here: 28 | [https://drive.google.com/file/d/10S0RJzY4s5fhVMPuxOFW1nxVQfUfJFIq/view?usp=sharing](https://drive.google.com/file/d/10S0RJzY4s5fhVMPuxOFW1nxVQfUfJFIq/view?usp=sharing) 29 | 30 | Once downloaded you will have to spin up the following ROS services, enter each of these commands in __a seperate terminal window__: 31 | ``` 32 | # Terminal 1 33 | roscore 34 | # Terminal 2 35 | rviz -d visualization.rviz 36 | # Terminal 3 37 | ./bin/slam_frontend -input=00098_2019-03-21-11-39-38.bag -visualize=true -output=output.bag 38 | ``` 39 | 40 | This will run the program and you will get something that looks like the following in your RViz window: 41 | 42 | ![The slam frontend in RViz with the pointcloud generated and the image currently seen.](img/Finished.png) 43 | 44 | __Note__, this is just a frontend, so the path might not match the one taken in the bag. This is the point of having a SLAM backend to perform optimization and correct for odometry drift. 45 | 46 | ### Arguments: 47 | 48 | The frontend supports the following arguments, only input and output are required. 49 | 50 | - __input__: This argument is a string, it is the relative path to the input bag that contains the stereo camera images and odometry. 51 | - __output__: This argument is a string, it is the name of the bag to output the ROS messages generated for the backend into. 52 | - __visualize__: This argument is a boolean, should a window open to show the current left camera image? 53 | - __save_debug__: This argument is a boolean, should we save debug images? 54 | - __odom_topic__: This argument is a string, the name of the odometry topic to use, should be of type `nav_msgs/Odometry`. 55 | - __left_image_topic__: This argument is a string, the name of the left camera image topic, should be of type `sensor_msgs/CompressedImage`. 56 | - __right_image_topic__: This argument is a string, the name of the right camera image topic, should be of type `sensor_msgs/CompressedImage`. 57 | - __max_poses__: This argument is an integer, it is the maximum number of poses to process, you can set this lower than the whole bag number of poses to process just the beginning. 58 | 59 | __Note__, if you are having trouble finding your image topics, use the `rosbag info ` command to find the names of all the topics and the types are list after the colon on the right. 60 | -------------------------------------------------------------------------------- /src/gui_helpers.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 joydeepb@cs.umass.edu 2 | // College of Information and Computer Sciences, 3 | // University of Massachusetts Amherst 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | #include "eigen3/Eigen/Dense" 26 | 27 | #include "glog/logging.h" 28 | #include "ros/ros.h" 29 | #include "visualization_msgs/Marker.h" 30 | #include "visualization_msgs/MarkerArray.h" 31 | 32 | #ifndef GUI_HELPERS_H 33 | #define GUI_HELPERS_H 34 | 35 | namespace gui_helpers { 36 | 37 | // RGBA color with each channel scaling from 0.0 to 1.0. 38 | struct Color4f { 39 | float r; 40 | float g; 41 | float b; 42 | float a; 43 | Color4f() {} 44 | Color4f(float r, float g, float b, float a) : r(r), g(g), b(b), a(a) {} 45 | 46 | static const Color4f kRed; 47 | static const Color4f kGreen; 48 | static const Color4f kBlue; 49 | static const Color4f kWhite; 50 | static const Color4f kBlack; 51 | static const Color4f kYellow; 52 | static const Color4f kCyan; 53 | static const Color4f kMagenta; 54 | }; 55 | 56 | // Initialize the marker to use the global "map" coordinate frame, identity 57 | // pose, and specified color and type. 58 | void InitializeMarker(int marker_type, 59 | const Color4f& color, 60 | float scale_x, 61 | float scale_y, 62 | float scale_z, 63 | visualization_msgs::Marker* msg); 64 | 65 | template 66 | geometry_msgs::Point StdPoint(const Vector3& v) { 67 | geometry_msgs::Point p; 68 | p.x = v.x(); 69 | p.y = v.y(); 70 | p.z = v.z(); 71 | return p; 72 | } 73 | 74 | template 75 | std_msgs::ColorRGBA StdColor(const Color& color) { 76 | std_msgs::ColorRGBA std_color; 77 | std_color.a = color.a; 78 | std_color.r = color.r; 79 | std_color.g = color.g; 80 | std_color.b = color.b; 81 | return std_color; 82 | } 83 | 84 | template 85 | void AddLine(const Vector3& v1, 86 | const Vector3& v2, 87 | const Color4f& color, 88 | visualization_msgs::Marker* msg) { 89 | CHECK_EQ(msg->type, visualization_msgs::Marker::LINE_LIST); 90 | msg->points.push_back(StdPoint(v1)); 91 | msg->points.push_back(StdPoint(v2)); 92 | msg->colors.push_back(StdColor(color)); 93 | msg->colors.push_back(StdColor(color)); 94 | } 95 | 96 | template 97 | void AddPoint(const Vector3& v, 98 | const Color4f& color, 99 | visualization_msgs::Marker* msg) { 100 | CHECK_EQ(msg->type, visualization_msgs::Marker::POINTS); 101 | msg->points.push_back(StdPoint(v)); 102 | msg->colors.push_back(StdColor(color)); 103 | } 104 | 105 | inline void ClearMarker(visualization_msgs::Marker* msg) { 106 | msg->points.clear(); 107 | msg->colors.clear(); 108 | } 109 | 110 | } // namespace gui_helpers 111 | 112 | #endif // GUI_HELPERS_H 113 | -------------------------------------------------------------------------------- /src/slam_to_ros.h: -------------------------------------------------------------------------------- 1 | //======================================================================== 2 | // This software is free: you can redistribute it and/or modify 3 | // it under the terms of the GNU Lesser General Public License Version 3, 4 | // as published by the Free Software Foundation. 5 | // 6 | // This software is distributed in the hope that it will be useful, 7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | // GNU Lesser General Public License for more details. 10 | // 11 | // You should have received a copy of the GNU Lesser General Public License 12 | // Version 3 in the file COPYING that came with this distribution. 13 | // If not, see . 14 | //======================================================================== 15 | /*! 16 | \file slam_frontend.cc 17 | \brief Slam types to ros message conversions 18 | \author Joydeep Biswas, (C) 2019 19 | */ 20 | //======================================================================== 21 | 22 | #ifndef __SLAM_TO_ROS_H__ 23 | #define __SLAM_TO_ROS_H__ 24 | 25 | #include "vision_slam_frontend/RobotPose.h" 26 | #include "vision_slam_frontend/SLAMNode.h" 27 | #include "vision_slam_frontend/FeatureMatch.h" 28 | #include "vision_slam_frontend/VisionFeature.h" 29 | #include "vision_slam_frontend/VisionFactor.h" 30 | #include "vision_slam_frontend/SLAMProblem.h" 31 | #include "vision_slam_frontend/OdometryFactor.h" 32 | #include "slam_types.h" 33 | #include "geometry_msgs/Quaternion.h" 34 | #include "geometry_msgs/Vector3.h" 35 | 36 | static vision_slam_frontend::FeatureMatch 37 | FeatureMatchToRos(const slam_types::FeatureMatch& pair) { 38 | vision_slam_frontend::FeatureMatch ros_pair; 39 | ros_pair.id_current = pair.feature_idx_current; 40 | ros_pair.id_initial = pair.feature_idx_initial; 41 | return ros_pair; 42 | } 43 | 44 | static vision_slam_frontend::VisionFeature 45 | VisionFeatureToRos(const slam_types::VisionFeature& feature) { 46 | vision_slam_frontend::VisionFeature ros_feature; 47 | ros_feature.pixel.x = feature.pixel.x(); 48 | ros_feature.pixel.y = feature.pixel.y(); 49 | ros_feature.pixel.z = 0; 50 | ros_feature.point3d.x = feature.point3d.x(); 51 | ros_feature.point3d.y = feature.point3d.y(); 52 | ros_feature.point3d.z = feature.point3d.z(); 53 | ros_feature.id = feature.feature_idx; 54 | // CHECK(std::isfinite(ros_feature.point3d.x)); 55 | // CHECK(std::isfinite(ros_feature.point3d.y)); 56 | // CHECK(std::isfinite(ros_feature.point3d.z)); 57 | return ros_feature; 58 | } 59 | 60 | static vision_slam_frontend::RobotPose 61 | RobotPoseToRos(const slam_types::RobotPose& rp) { 62 | vision_slam_frontend::RobotPose ros_rp; 63 | ros_rp.loc.x = rp.loc[0]; 64 | ros_rp.loc.y = rp.loc[1]; 65 | ros_rp.loc.z = rp.loc[2]; 66 | ros_rp.angle.w = rp.angle.w(); 67 | ros_rp.angle.x = rp.angle.x(); 68 | ros_rp.angle.y = rp.angle.y(); 69 | ros_rp.angle.z = rp.angle.z(); 70 | return ros_rp; 71 | } 72 | 73 | static vision_slam_frontend::SLAMNode 74 | SLAMNodeToRos(const slam_types::SLAMNode& node) { 75 | vision_slam_frontend::SLAMNode ros_node; 76 | ros_node.id = node.node_idx; 77 | ros_node.timestamp = node.timestamp; 78 | ros_node.pose = RobotPoseToRos(node.pose); 79 | for (auto p : node.features) { 80 | ros_node.features.push_back(VisionFeatureToRos(p)); 81 | } 82 | return ros_node; 83 | } 84 | 85 | static vision_slam_frontend::VisionFactor 86 | VisionFactorToRos(const slam_types::VisionFactor& corr) { 87 | vision_slam_frontend::VisionFactor ros_corr; 88 | ros_corr.pose_current = corr.pose_idx_current; 89 | ros_corr.pose_initial = corr.pose_idx_initial; 90 | for (auto m : corr.feature_matches) { 91 | ros_corr.feature_matches.push_back(FeatureMatchToRos(m)); 92 | } 93 | return ros_corr; 94 | } 95 | 96 | static vision_slam_frontend::OdometryFactor 97 | OdometryFactorToRos(const slam_types::OdometryFactor& odom_f) { 98 | vision_slam_frontend::OdometryFactor ros_odom_f; 99 | ros_odom_f.pose_i = odom_f.pose_i; 100 | ros_odom_f.pose_j = odom_f.pose_j; 101 | ros_odom_f.rotation.w = odom_f.rotation.w(); 102 | ros_odom_f.rotation.x = odom_f.rotation.x(); 103 | ros_odom_f.rotation.y = odom_f.rotation.y(); 104 | ros_odom_f.rotation.z = odom_f.rotation.z(); 105 | ros_odom_f.translation.x = odom_f.translation[0]; 106 | ros_odom_f.translation.y = odom_f.translation[1]; 107 | ros_odom_f.translation.z = odom_f.translation[2]; 108 | return ros_odom_f; 109 | } 110 | 111 | static vision_slam_frontend::SLAMProblem 112 | SLAMProblemToRos(const slam_types::SLAMProblem& problem) { 113 | vision_slam_frontend::SLAMProblem ros_problem; 114 | for (auto node : problem.nodes) { 115 | ros_problem.nodes.push_back(SLAMNodeToRos(node)); 116 | } 117 | for (auto vfactor : problem.vision_factors) { 118 | ros_problem.vision_factors.push_back(VisionFactorToRos(vfactor)); 119 | } 120 | for (auto ofactor : problem.odometry_factors) { 121 | ros_problem.odometry_factors.push_back(OdometryFactorToRos(ofactor)); 122 | } 123 | return ros_problem; 124 | } 125 | 126 | vision_slam_frontend::CameraIntrinsics 127 | IntrinsicsToRos(const slam_types::CameraIntrinsics& k) { 128 | vision_slam_frontend::CameraIntrinsics ros_k; 129 | ros_k.fx = k.fx; 130 | ros_k.cx = k.cx; 131 | ros_k.fy = k.fy; 132 | ros_k.cy = k.cy; 133 | return ros_k; 134 | } 135 | 136 | vision_slam_frontend::CameraExtrinsics 137 | ExtrinsicsToRos(const slam_types::CameraExtrinsics& a) { 138 | vision_slam_frontend::CameraExtrinsics ros_a; 139 | for (int i = 0; i < 3; ++i) { 140 | ros_a.translation[i] = a.translation[i]; 141 | ros_a.rotation[i] = a.rotation[i]; 142 | } 143 | return ros_a; 144 | } 145 | 146 | #endif // __SLAM_TO_ROS_H__ 147 | -------------------------------------------------------------------------------- /visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Frontend 3D1 10 | - /Backend 3D1 11 | - /Backend Poses1 12 | Splitter Ratio: 0.5 13 | Tree Height: 442 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Stereo Matches 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/MarkerArray 57 | Enabled: true 58 | Marker Topic: /slam_frontend/pose_graph 59 | Name: Pose Graph 60 | Namespaces: 61 | "": true 62 | Queue Size: 100 63 | Value: true 64 | - Class: rviz/Image 65 | Enabled: false 66 | Image Topic: /slam_frontend/debug_image 67 | Max Value: 1 68 | Median window: 5 69 | Min Value: 0 70 | Name: Feature matches 71 | Normalize Range: true 72 | Queue Size: 2 73 | Transport Hint: raw 74 | Unreliable: false 75 | Value: false 76 | - Class: rviz/Marker 77 | Enabled: true 78 | Marker Topic: /slam_frontend/points 79 | Name: Frontend 3D 80 | Namespaces: 81 | "": true 82 | Queue Size: 100 83 | Value: true 84 | - Class: rviz/Image 85 | Enabled: false 86 | Image Topic: /slam_frontend/debug_stereo_image 87 | Max Value: 1 88 | Median window: 5 89 | Min Value: 0 90 | Name: Stereo Matches 91 | Normalize Range: true 92 | Queue Size: 2 93 | Transport Hint: raw 94 | Unreliable: false 95 | Value: false 96 | - Class: rviz/Image 97 | Enabled: true 98 | Image Topic: /slam_frontend/debug_image 99 | Max Value: 1 100 | Median window: 5 101 | Min Value: 0 102 | Name: Visual Flow 103 | Normalize Range: true 104 | Queue Size: 2 105 | Transport Hint: raw 106 | Unreliable: false 107 | Value: true 108 | - Class: rviz/Marker 109 | Enabled: true 110 | Marker Topic: /slam_map 111 | Name: Backend 3D 112 | Namespaces: 113 | "": true 114 | Queue Size: 100 115 | Value: true 116 | - Class: rviz/Marker 117 | Enabled: true 118 | Marker Topic: /slam_poses 119 | Name: Backend Poses 120 | Namespaces: 121 | "": true 122 | Queue Size: 100 123 | Value: true 124 | Enabled: true 125 | Global Options: 126 | Background Color: 48; 48; 48 127 | Default Light: true 128 | Fixed Frame: map 129 | Frame Rate: 30 130 | Name: root 131 | Tools: 132 | - Class: rviz/Interact 133 | Hide Inactive Objects: true 134 | - Class: rviz/MoveCamera 135 | - Class: rviz/Select 136 | - Class: rviz/FocusCamera 137 | - Class: rviz/Measure 138 | - Class: rviz/SetInitialPose 139 | Topic: /initialpose 140 | - Class: rviz/SetGoal 141 | Topic: /move_base_simple/goal 142 | - Class: rviz/PublishPoint 143 | Single click: true 144 | Topic: /clicked_point 145 | Value: true 146 | Views: 147 | Current: 148 | Class: rviz/Orbit 149 | Distance: 11.044445991516113 150 | Enable Stereo Rendering: 151 | Stereo Eye Separation: 0.05999999865889549 152 | Stereo Focal Distance: 1 153 | Swap Stereo Eyes: false 154 | Value: false 155 | Focal Point: 156 | X: 1.4752784967422485 157 | Y: 1.2086460590362549 158 | Z: -0.7158879637718201 159 | Focal Shape Fixed Size: true 160 | Focal Shape Size: 0.05000000074505806 161 | Invert Z Axis: false 162 | Name: Current View 163 | Near Clip Distance: 0.009999999776482582 164 | Pitch: 0.9347972273826599 165 | Target Frame: 166 | Value: Orbit (rviz) 167 | Yaw: 3.1182620525360107 168 | Saved: ~ 169 | Window Geometry: 170 | Displays: 171 | collapsed: false 172 | Feature matches: 173 | collapsed: false 174 | Height: 1178 175 | Hide Left Dock: false 176 | Hide Right Dock: true 177 | QMainWindow State: 000000ff00000000fd0000000400000000000002310000040bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003600000240000000bc00fffffffb0000001c00530074006500720065006f0020004d00610074006300680065007302000004ef00000346000001cd00000186fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e00460065006100740075007200650020006d00610074006300680065007302000004b10000017d000003f30000029bfb0000001600560069007300750061006c00200046006c006f0077010000027b000001c60000001600ffffff000000010000010f00000409fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000035000004090000009900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e00000039fc0100000002fb0000000800540069006d006501000000000000077e0000023a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005480000040b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 178 | Selection: 179 | collapsed: false 180 | Stereo Matches: 181 | collapsed: false 182 | Time: 183 | collapsed: false 184 | Tool Properties: 185 | collapsed: false 186 | Views: 187 | collapsed: true 188 | Visual Flow: 189 | collapsed: false 190 | Width: 1918 191 | X: 1200 192 | Y: 380 193 | -------------------------------------------------------------------------------- /src/math_util.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 - 2018 kvedder@umass.edu, joydeepb@cs.umass.edu, 2 | // slane@cs.umass.edu 3 | // College of Information and Computer Sciences, 4 | // University of Massachusetts Amherst 5 | // 6 | // 7 | // This software is free: you can redistribute it and/or modify 8 | // it under the terms of the GNU Lesser General Public License Version 3, 9 | // as published by the Free Software Foundation. 10 | // 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU Lesser General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU Lesser General Public License 17 | // Version 3 in the file COPYING that came with this distribution. 18 | // If not, see . 19 | // ======================================================================== 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "glog/logging.h" 26 | 27 | #ifndef SRC_MATH_MATH_UTIL_H_ 28 | #define SRC_MATH_MATH_UTIL_H_ 29 | 30 | #define M_2PI (2.0 * M_PI) 31 | 32 | namespace math_util { 33 | 34 | // Convert angle in radians to degrees. 35 | template 36 | T Clamp(const T value, const T min, const T max) { 37 | return std::max(min, std::min(value, max)); 38 | } 39 | 40 | // Convert angle in radians to degrees. 41 | template 42 | T RadToDeg(T angle) { 43 | return (angle / M_PI * 180.0); 44 | } 45 | 46 | // Convert angle in degrees to radians. 47 | template 48 | constexpr T DegToRad(T angle) { 49 | return (angle / 180.0 * M_PI); 50 | } 51 | 52 | template 53 | T AngleMod(T angle) { 54 | angle -= M_2PI * rint(angle / M_2PI); 55 | return angle; 56 | } 57 | 58 | template 59 | T AngleDiff(T a0, T a1) { 60 | return AngleMod(a0 - a1); 61 | } 62 | 63 | template 64 | T AngleDist(T a0, T a1) { 65 | return std::fabs(AngleMod(a0 - a1)); 66 | } 67 | 68 | template 69 | T Sq(const T& x) { 70 | return (x * x); 71 | } 72 | 73 | template 74 | T Cube(const T& x) { 75 | return (x * x * x); 76 | } 77 | 78 | template 79 | T Pow(const T& x) { 80 | T y = static_cast(1); 81 | for (unsigned int i = 0; i < n; ++i) { 82 | y *= x; 83 | } 84 | return y; 85 | } 86 | 87 | template 88 | T Pow(const T& x, const unsigned int n) { 89 | T y = static_cast(1); 90 | for (unsigned int i = 0; i < n; ++i) { 91 | y *= x; 92 | } 93 | return y; 94 | } 95 | 96 | // Take given base to a given power as a constexpr. 97 | template 98 | constexpr T ConstexprPow(const T& base, const int exp, const T result = 1) { 99 | return (exp < 1) ? result 100 | : ConstexprPow((base * base), (exp / 2), 101 | ((exp % 2) ? result * base : result)); 102 | } 103 | 104 | // Return the truncated linearly interpolated value between y_min and y_max 105 | // depending on the provided value of x, x_min, and x_max. 106 | template 107 | T Ramp(const T x, const T x_min, const T x_max, const T y_min, const T y_max) { 108 | if (x <= x_min) { 109 | return y_min; 110 | } else if (x >= x_max) { 111 | return y_max; 112 | } 113 | return (y_min + (x - x_min) / (x_max - x_min) * (y_max - y_min)); 114 | } 115 | 116 | // Solve the quadratic polynomial a * x^2 + b * x + c = 0, and return the 117 | // two real roots as r0 and r1, where r0 is less than or equal to r1. The return 118 | // value is the number of unique real roots found. 119 | template 120 | unsigned int SolveQuadratic(const T& a, const T& b, const T& c, T* r0, T* r1) { 121 | DCHECK_NE(r0, static_cast(nullptr)); 122 | DCHECK_NE(r1, static_cast(nullptr)); 123 | const T discriminant = Sq(b) - T(4.0) * a * c; 124 | if (discriminant < T(0)) { 125 | return 0; 126 | } else if (discriminant == 0) { 127 | *r0 = *r1 = -b / (T(2.0) * a); 128 | return 1; 129 | } 130 | const T sqrt_discriminant = sqrt(discriminant); 131 | if (a >= T(0)) { 132 | *r0 = (-b - sqrt_discriminant) / (2.0 * a); 133 | *r1 = (-b + sqrt_discriminant) / (2.0 * a); 134 | } else { 135 | *r0 = (-b + sqrt_discriminant) / (2.0 * a); 136 | *r1 = (-b - sqrt_discriminant) / (2.0 * a); 137 | } 138 | return 2; 139 | } 140 | 141 | // returns the +/- sign of a numerical value, 0 if value is 0 142 | template 143 | int Sign(T val) { 144 | return (T(0) < val) - (val < T(0)); 145 | } 146 | 147 | // Solve the cubic polynomial a * x^3 + b * x^2 + c * x + d = 0, and return the 148 | // real roots as r0, r1, and r2 where r0 is the smallest real root, and r2 is 149 | // the largest real root. The return value is the number of unique real roots 150 | // found. 151 | template 152 | unsigned int SolveCubic(const T& a, const T& b, const T& c, const T& d, T* r0, 153 | T* r1, T* r2) { 154 | DCHECK_NE(r0, static_cast(nullptr)); 155 | DCHECK_NE(r1, static_cast(nullptr)); 156 | DCHECK_NE(r2, static_cast(nullptr)); 157 | const T discriminant = T(18) * a * b * c * d - T(4) * Cube(b) * d + 158 | Sq(b) * Sq(c) - T(4) * a * Cube(c) - 159 | T(27) * Sq(a) * Sq(d); 160 | 161 | const T discriminant0 = Sq(b) - T(3) * a * c; 162 | 163 | const T discriminant1 = 164 | T(2) * Cube(b) - T(9) * a * b * c + T(27) * Sq(a) * d; 165 | 166 | if (discriminant > T(0)) { 167 | // todo(slane): this case is not working 168 | // Three distinct real roots 169 | const T p = (T(3) * a * c - Sq(b)) / (3 * Sq(a)); 170 | const T q = (T(2) * Cube(b) - 9 * a * b * c + 27 * Sq(a) * d) / 171 | (T(27) * Cube(a)); 172 | 173 | std::array roots; 174 | 175 | for (T k = 0; k < 3; k++) { 176 | roots[k] = (T(2) * sqrt(-p / T(3)) * 177 | cos(T(1.0 / 3.0) * 178 | acos(T(3) * q / (T(2) * p) * sqrt(T(-3) / p)) - 179 | T(2.0) * k * M_PI / T(3.0)) - 180 | b / (T(3) * a)); 181 | } 182 | 183 | *r0 = roots[2]; 184 | *r1 = roots[1]; 185 | *r2 = roots[0]; 186 | 187 | return 3; 188 | } else if (discriminant == T(0)) { 189 | if (discriminant0 == 0) { 190 | *r2 = -b / (T(3) * a); 191 | return 1; 192 | } else { 193 | const T double_root = (T(9) * a * d - b * c) / (T(2) * discriminant0); 194 | 195 | const T simple_root = (T(4) * a * b * c - 9 * Sq(a) * d - Cube(b)) / 196 | (a * discriminant0); 197 | 198 | if (double_root > simple_root) { 199 | *r2 = double_root; 200 | *r1 = simple_root; 201 | } else { 202 | *r2 = simple_root; 203 | *r1 = double_root; 204 | } 205 | return 2; 206 | } 207 | } else { 208 | // One real root and two non-real complex conjugate roots. 209 | T C = cbrt(((discriminant1 + sqrt(T(-27) * Sq(a) * discriminant)) / 2)); 210 | 211 | if (C == 0) { 212 | C = cbrt(((discriminant1 - sqrt(T(-27) * Sq(a) * discriminant)) / 2)); 213 | } 214 | 215 | *r2 = -T(1) / (T(3) * a) * (b + C + discriminant0 / C); 216 | 217 | return 1; 218 | } 219 | } 220 | 221 | } // namespace math_util 222 | 223 | #endif // SRC_MATH_MATH_UTIL_H_ 224 | -------------------------------------------------------------------------------- /src/slam_frontend.h: -------------------------------------------------------------------------------- 1 | //======================================================================== 2 | // This software is free: you can redistribute it and/or modify 3 | // it under the terms of the GNU Lesser General Public License Version 3, 4 | // as published by the Free Software Foundation. 5 | // 6 | // This software is distributed in the hope that it will be useful, 7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | // GNU Lesser General Public License for more details. 10 | // 11 | // You should have received a copy of the GNU Lesser General Public License 12 | // Version 3 in the file COPYING that came with this distribution. 13 | // If not, see . 14 | //======================================================================== 15 | /*! 16 | \file slam_frontend.h 17 | \brief A vision SLAM frontend 18 | \author Joydeep Biswas, (C) 2018 19 | */ 20 | //======================================================================== 21 | 22 | #ifndef __SLAM_FRONTEND_H__ 23 | #define __SLAM_FRONTEND_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "eigen3/Eigen/Dense" 31 | #include "eigen3/Eigen/Geometry" 32 | #include "opencv2/opencv.hpp" 33 | 34 | #include "slam_types.h" 35 | 36 | namespace slam { 37 | 38 | // Pinhole camera intrinsics parameters. 39 | // The convention used here matches that of OpenCV: 40 | // https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/ 41 | // camera_calibration.html 42 | struct CameraIntrinsics { 43 | // Radial distortion parameters. 44 | float k1, k2, k3; 45 | // Tangential distortion parameters. 46 | float p1, p2; 47 | // Focal length x. 48 | float fx; 49 | // Focal length y. 50 | float fy; 51 | // Principal point x. 52 | float cx; 53 | // Principal point y. 54 | float cy; 55 | }; 56 | 57 | /* A container for slam configuration data */ 58 | struct FrontendConfig { 59 | public: 60 | enum class DescriptorExtractorType { 61 | AKAZE, 62 | ORB, 63 | BRISK, 64 | SURF, 65 | SIFT, 66 | FREAK 67 | }; 68 | FrontendConfig(); 69 | void Load(const std::string& path); 70 | 71 | bool debug_images_; 72 | DescriptorExtractorType descriptor_extract_type_; 73 | float best_percent_; 74 | float nn_match_ratio_; 75 | // If the robot translates by this much, as reported by odometry, the next 76 | // image will be used for a vision frame. 77 | float min_odom_translation; 78 | // If the robot rotates by this much, as reported by odometry, the next 79 | // image will be used for a vision frame. 80 | float min_odom_rotation; 81 | // The minimum number of feature matches that must exist between a pair of 82 | // frames to add a vision correspondence feature. 83 | uint32_t min_vision_matches; 84 | // The maximum feature track length -- the maximum number of pose-steps older 85 | // than the current pose that should be compared for vision factors. 86 | uint32_t frame_life_; 87 | // Feature matching norm type (e.g. L2, Hamming, etc.). 88 | cv::NormTypes bf_matcher_param_; 89 | // Camera intrinsics. 90 | CameraIntrinsics intrinsics_left, intrinsics_right; 91 | // Derived parameters, computed from intrinsics. 92 | cv::Mat camera_matrix_left, camera_matrix_right, distortion_coeffs_left, 93 | distortion_coeffs_right, projection_left, projection_right; 94 | Eigen::Matrix3f fundamental; 95 | // Affine transforms from frame of camera to robot. 96 | Eigen::Affine3f left_cam_to_robot; 97 | }; 98 | 99 | /* A container for slam node data */ 100 | class Frame { 101 | public: 102 | Frame(const std::vector& keypoints, 103 | const cv::Mat& descriptors, 104 | uint64_t frame_ID); 105 | Frame() {} 106 | uint64_t frame_ID_; 107 | std::vector keypoints_; 108 | std::vector is_initial_; 109 | std::vector initial_ids_; 110 | cv::Mat descriptors_; 111 | std::unordered_map> initial_appearances; 113 | cv::Mat debug_image_; 114 | }; 115 | 116 | /* The actual processing unit for the entire frontend */ 117 | class Frontend { 118 | public: 119 | explicit Frontend(const std::string& config_path); 120 | // Observe a new image. Extract features, and match to past frames. Returns 121 | // true iff a new SLAM node is added to the SLAM problem. 122 | bool ObserveImage(const cv::Mat& left_image, 123 | const cv::Mat& right_image, 124 | double time); 125 | // Observe new odometry message. 126 | void ObserveOdometry(const Eigen::Vector3f& translation, 127 | const Eigen::Quaternionf& rotation, 128 | double timestamp); 129 | std::vector getDebugImages(); 130 | // Return the latest debug image. 131 | cv::Mat GetLastDebugImage(); 132 | cv::Mat GetLastDebugStereoImage(); 133 | std::vector getDebugStereoImages(); 134 | 135 | // Get a fully instantiated SLAM problem with the data collected so far. 136 | void GetSLAMProblem(slam_types::SLAMProblem* problem) const; 137 | 138 | // Get the number od poses (SLAM nodes) added so far. 139 | int GetNumPoses(); 140 | 141 | // Get the frontend configuration parameters. 142 | FrontendConfig GetConfig() { return config_; } 143 | 144 | private: 145 | // Returns true iff odometry reports that the robot has moved sufficiently to 146 | // warrant a vision update. 147 | bool OdomCheck(); 148 | // Extract feature descriptors from image, and construct a Vision frame. 149 | void ExtractFeatures(cv::Mat image, Frame* curr_frame); 150 | // Return feature matches between frame1 and frame2, and also update frame2 151 | // to track the initial frame for all matches. 152 | slam_types::VisionFactor* GetFeatureMatches(Frame* past_frame_ptr, 153 | Frame* curr_frame_ptr); 154 | // Returns the matches between frame_query and frame_train 155 | std::vector GetMatches(const Frame& frame_query, 156 | const Frame& frame_train, 157 | double nn_match_ratio); 158 | void RemoveAmbigStereo(Frame* left, 159 | Frame* right, 160 | const std::vector stereo_matches); 161 | // Create a new odometry factor, and reset odometry tracking variables. 162 | void AddOdometryFactor(); 163 | // Removes radial distortion from all observed feature points. 164 | void UndistortFeaturePoints(std::vector* features); 165 | // Takes the stereo images of the same scene and uses the corresponding 166 | // matches to get the depth estimate of the 3D point. 167 | void Calculate3DPoints(Frame* left_frame, 168 | Frame* right_frame, 169 | std::vector* points); 170 | 171 | // Indicates if odometry has been initialized or not. 172 | bool odom_initialized_; 173 | // Initial odometry translation. 174 | Eigen::Vector3f init_odom_translation_; 175 | // Initial odometry rotation. 176 | Eigen::Quaternionf init_odom_rotation_; 177 | // Previous odometry-reported pose translation. 178 | Eigen::Vector3f prev_odom_translation_; 179 | // Previous odometry-reported pose rotation. 180 | Eigen::Quaternionf prev_odom_rotation_; 181 | // Latest odometry-reported pose translation. 182 | Eigen::Vector3f odom_translation_; 183 | // Latest odometry-reported pose rotation. 184 | Eigen::Quaternionf odom_rotation_; 185 | // Latest odometry timestamp. 186 | double odom_timestamp_; 187 | // Configuration parameters for frontend. 188 | FrontendConfig config_; 189 | // Brute-force matcher for descriptor matching 190 | cv::Ptr matcher_; 191 | // Next frame ID. 192 | uint64_t curr_frame_ID_; 193 | std::vector frame_list_; 194 | cv::Ptr descriptor_extractor_; 195 | cv::Ptr fast_feature_detector_; 196 | // Formatted data for slam problem 197 | std::vector vision_factors_; 198 | std::vector nodes_; 199 | std::vector odometry_factors_; 200 | 201 | // Debug 202 | std::vector debug_images_; 203 | std::vector debug_stereo_images_; 204 | }; 205 | } // namespace slam 206 | 207 | #endif // __SLAM_FRONTEND_H__ 208 | -------------------------------------------------------------------------------- /src/slam_types.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 joydeepb@cs.umass.edu 2 | // College of Information and Computer Sciences, 3 | // University of Massachusetts Amherst 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef __SLAM_TYPES_H__ 24 | #define __SLAM_TYPES_H__ 25 | 26 | #include 27 | #include 28 | 29 | #include "eigen3/Eigen/Dense" 30 | #include "eigen3/Eigen/Geometry" 31 | 32 | namespace slam_types { 33 | 34 | 35 | // Pinhole camera intrinsics parameters. 36 | // The convention used here matches that of OpenCV: 37 | // https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/ 38 | // camera_calibration.html 39 | struct CameraIntrinsics { 40 | // Focal length x. 41 | float fx; 42 | // Focal length y. 43 | float fy; 44 | // Principal point x. 45 | float cx; 46 | // Principal point y. 47 | float cy; 48 | }; 49 | 50 | // The camera extrinsics consists of the coordinate transform from the 51 | // camera to the robot pose. That is, it consists of the translation and 52 | // rotation that takes a point from the camera frame to the robot frame. 53 | struct CameraExtrinsics { 54 | // 3D vector of translation. 55 | float translation[3]; 56 | // Rotation in scaled angle-axis (Lie algebra) form. 57 | float rotation[3]; 58 | }; 59 | 60 | struct VisionFeature { 61 | // Index of this feature in the features vector for the node, stored here 62 | // for redundancy. 63 | uint64_t feature_idx; 64 | // Camera pixel location of feature. 65 | Eigen::Vector2f pixel; 66 | // Estimated 3d location in the camera frame. 67 | Eigen::Vector3f point3d; 68 | // Default constructor: do nothing. 69 | VisionFeature() {} 70 | // Convenience constructor: initialize everything. 71 | VisionFeature(uint64_t idx, 72 | const Eigen::Vector2f& p, 73 | const Eigen::Vector3f& point3d) : 74 | feature_idx(idx), pixel(p), point3d(point3d) {} 75 | }; 76 | 77 | struct FeatureMatch { 78 | // Feature ID from the initial pose. 79 | uint64_t feature_idx_initial; 80 | // Feature ID from current pose. 81 | uint64_t feature_idx_current; 82 | // Default constructor: do nothing. 83 | FeatureMatch() {} 84 | // Convenience constructor: initialize everything. 85 | FeatureMatch(uint64_t fid_initial, 86 | uint64_t fid_current) : 87 | feature_idx_initial(fid_initial), 88 | feature_idx_current(fid_current) {} 89 | }; 90 | 91 | struct VisionFactor { 92 | // ID of the pose where the features were *first* observed. 93 | uint64_t pose_idx_initial; 94 | // ID of second pose. 95 | uint64_t pose_idx_current; 96 | // Pair of feature ID from first pose, and feature ID from second pose, 97 | // and feature ID from initial pose. 98 | std::vector feature_matches; 99 | // Default constructor: do nothing. 100 | VisionFactor() {} 101 | // Convenience constructor: initialize everything. 102 | VisionFactor( 103 | uint64_t pose_initial, 104 | uint64_t pose_current, 105 | const std::vector& feature_matches) : 106 | pose_idx_initial(pose_initial), pose_idx_current(pose_current), 107 | feature_matches(feature_matches) {} 108 | }; 109 | 110 | struct RobotPose { 111 | // Robot location. 112 | Eigen::Vector3f loc; 113 | // Robot angle: rotates points from robot frame to global. 114 | Eigen::Quaternionf angle; 115 | // Default constructor: do nothing. 116 | RobotPose() {} 117 | // Convenience constructor: initialize everything. 118 | RobotPose(const Eigen::Vector3f& loc, 119 | const Eigen::Quaternionf& angle) : 120 | loc(loc), angle(angle) {} 121 | // Return a transform from the robot to the world frame for this pose. 122 | Eigen::Affine3f RobotToWorldTf() const { 123 | return (Eigen::Translation3f(loc) * angle); 124 | } 125 | // Return a transform from the world to the robot frame for this pose. 126 | Eigen::Affine3f WorldToRobotTf() const { 127 | return ((Eigen::Translation3f(loc) * angle).inverse()); 128 | } 129 | }; 130 | 131 | // Represents an IMU or odometry, or fused IMU-odometry observation. 132 | struct OdometryFactor { 133 | // ID of first pose. 134 | uint64_t pose_i; 135 | // ID of second pose. 136 | uint64_t pose_j; 137 | // Translation to go from pose i to pose j. 138 | Eigen::Vector3f translation; 139 | // Rotation to go from pose i to pose j. 140 | Eigen::Quaternionf rotation; 141 | // Default constructor: do nothing. 142 | OdometryFactor() {} 143 | // Convenience constructor: initialize everything. 144 | OdometryFactor(uint64_t pose_i, 145 | uint64_t pose_j, 146 | Eigen::Vector3f translation, 147 | Eigen::Quaternionf rotation) : 148 | pose_i(pose_i), pose_j(pose_j), translation(translation), 149 | rotation(rotation) {} 150 | }; 151 | 152 | struct SLAMNode { 153 | // Pose index for this node in the nodes vector for the slam problem. 154 | uint64_t node_idx; 155 | // Timestamp. 156 | double timestamp; 157 | // 6DOF parameters. 158 | RobotPose pose; 159 | // Observed vision features. 160 | std::vector features; 161 | // Default constructor: do nothing. 162 | SLAMNode() {} 163 | // Convenience constructor, initialize all components. 164 | SLAMNode(uint64_t idx, 165 | double timestamp, 166 | const RobotPose& pose, 167 | const std::vector& features) : 168 | node_idx(idx), timestamp(timestamp), pose(pose), features(features) {} 169 | }; 170 | 171 | struct SLAMProblem { 172 | // Nodes in the pose graph. 173 | std::vector nodes; 174 | // Vision correspondences. 175 | std::vector vision_factors; 176 | // Odometry / IMU correspondences. 177 | std::vector odometry_factors; 178 | // Default constructor, do nothing. 179 | SLAMProblem() {} 180 | // Convenience constructor for initialization. 181 | SLAMProblem(const std::vector& nodes, 182 | const std::vector& vision_factors, 183 | const std::vector& odometry_factors) : 184 | nodes(nodes), 185 | vision_factors(vision_factors), 186 | odometry_factors(odometry_factors) {} 187 | }; 188 | 189 | 190 | struct SLAMNodeSolution { 191 | // Pose ID. 192 | uint64_t node_idx; 193 | // Timestamp. 194 | double timestamp; 195 | // 6DOF parameters: tx, ty, tx, angle_x, angle_y, angle_z. Note that 196 | // angle_* are the coordinates in scaled angle-axis form. 197 | double pose[6]; 198 | // Observed vision feature inverse depths. 199 | std::vector inverse_depths; 200 | // Corresponding flag of whether the point is to be part of the map. 201 | std::vector point_in_map; 202 | // Convenience constructor, initialize all values. 203 | explicit SLAMNodeSolution(const SLAMNode& n) : 204 | node_idx(n.node_idx), 205 | timestamp(n.timestamp), 206 | pose{0, 0, 0, 0, 0, 0}, 207 | inverse_depths(n.features.size(), 1.0), 208 | point_in_map(n.features.size(), false) { 209 | const Eigen::AngleAxisf rotation(n.pose.angle); 210 | const Eigen::Vector3f rotation_aa = rotation.angle() * rotation.axis(); 211 | pose[0] = n.pose.loc.x(); 212 | pose[1] = n.pose.loc.y(); 213 | pose[2] = n.pose.loc.z(); 214 | pose[3] = rotation_aa.x(); 215 | pose[4] = rotation_aa.y(); 216 | pose[5] = rotation_aa.z()+0.02; 217 | } 218 | }; 219 | 220 | 221 | } // namespace slam_types 222 | 223 | #endif // __SLAM_TYPES_H__ 224 | -------------------------------------------------------------------------------- /src/slam_frontend_main.cc: -------------------------------------------------------------------------------- 1 | //======================================================================== 2 | // This software is free: you can redistribute it and/or modify 3 | // it under the terms of the GNU Lesser General Public License Version 3, 4 | // as published by the Free Software Foundation. 5 | // 6 | // This software is distributed in the hope that it will be useful, 7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | // GNU Lesser General Public License for more details. 10 | // 11 | // You should have received a copy of the GNU Lesser General Public License 12 | // Version 3 in the file COPYING that came with this distribution. 13 | // If not, see . 14 | //======================================================================== 15 | /*! 16 | \file slam_main.cc 17 | \brief Main entry point to process a ROS bag file through a vision SLAM 18 | frontend, and save the resulting SLAM dataset. 19 | \author Joydeep Biswas, (C) 2018 20 | */ 21 | //======================================================================== 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "glog/logging.h" 33 | #include "opencv2/opencv.hpp" 34 | #include "opencv2/imgcodecs.hpp" 35 | #include "gflags/gflags.h" 36 | #include "eigen3/Eigen/Dense" 37 | #include "eigen3/Eigen/Geometry" 38 | #include "image_transport/image_transport.h" 39 | #include "sensor_msgs/CompressedImage.h" 40 | #include "nav_msgs/Odometry.h" 41 | #include "rosbag/bag.h" 42 | #include "rosbag/view.h" 43 | #include "ros/package.h" 44 | #include "cv_bridge/cv_bridge.h" 45 | #include "visualization_msgs/Marker.h" 46 | #include "visualization_msgs/MarkerArray.h" 47 | #include "vision_slam_frontend/CameraIntrinsics.h" 48 | #include "vision_slam_frontend/CameraExtrinsics.h" 49 | 50 | #include "gui_helpers.h" 51 | #include "slam_frontend.h" 52 | #include "slam_to_ros.h" 53 | 54 | 55 | using gui_helpers::ClearMarker; 56 | using gui_helpers::Color4f; 57 | using gui_helpers::AddPoint; 58 | using gui_helpers::AddLine; 59 | using gui_helpers::InitializeMarker; 60 | using ros::Time; 61 | using slam::Frontend; 62 | using slam_types::RobotPose; 63 | using slam_types::SLAMNode; 64 | using slam_types::SLAMProblem; 65 | using slam_types::OdometryFactor; 66 | using slam_types::VisionFactor; 67 | using slam_types::FeatureMatch; 68 | using slam_types::VisionFeature; 69 | using std::cout; 70 | using std::string; 71 | using std::vector; 72 | using Eigen::Affine3f; 73 | using Eigen::AngleAxisf; 74 | using Eigen::Quaternionf; 75 | using Eigen::Translation3f; 76 | using Eigen::Vector3f; 77 | using Eigen::Matrix3f; 78 | using visualization_msgs::Marker; 79 | using visualization_msgs::MarkerArray; 80 | 81 | DEFINE_string(left_image_topic, 82 | "/stereo/left/image_raw/compressed", 83 | "Name of ROS topic for left camera images"); 84 | DEFINE_string(right_image_topic, 85 | "/stereo/right/image_raw/compressed", 86 | "Name of ROS topic for right camera images"); 87 | DEFINE_string(odom_topic, 88 | "/odometry/filtered", 89 | "Name of ROS topic for odometry data"); 90 | DEFINE_string(input, "", "Name of ROS bag file to load"); 91 | DEFINE_string(output, "", "Name of ROS bag file to output"); 92 | DEFINE_bool(visualize, false, "Display images loaded"); 93 | DEFINE_bool(save_debug, false, "Save debug images"); 94 | DEFINE_int32(max_poses, 0, "Maximum number of SLAM poses to process"); 95 | DECLARE_string(helpon); 96 | DECLARE_int32(v); 97 | 98 | cv::Mat DecodeImage(const sensor_msgs::CompressedImage& msg) { 99 | cv::Mat image = cv::imdecode(cv::InputArray(msg.data), 100 | cv::ImreadModes::IMREAD_GRAYSCALE); 101 | if (msg.format.find("bayer_rggb8") != string::npos) { 102 | vector image_channels(image.channels()); 103 | cv::split(image, image_channels.data()); 104 | image = image_channels[0]; 105 | cv::cvtColor(image_channels[0], image, cv::COLOR_BayerBG2BGR); 106 | cv::cvtColor(image, image, cv::COLOR_BGR2GRAY); 107 | } 108 | return image; 109 | } 110 | 111 | bool CompressedImageCallback(const std::pair< 112 | sensor_msgs::CompressedImage, 113 | sensor_msgs::CompressedImage>& img_pair, 114 | Frontend* frontend) { 115 | double image_time = img_pair.first.header.stamp.toSec(); 116 | if (FLAGS_v > 1) { 117 | printf("CompressedImage t=%f\n", image_time); 118 | } 119 | // Decode the left image 120 | // Process the left image 121 | // Then find the closest point in the left image. 122 | // Calculate the Depth and X Y and broadcast based on that. 123 | cv::Mat left_image = DecodeImage(img_pair.first); 124 | cv::Mat right_image = DecodeImage(img_pair.second); 125 | if (FLAGS_visualize) { 126 | cv::namedWindow("Display Image", cv::WINDOW_AUTOSIZE); 127 | cv::imshow("Display Image", left_image); 128 | if (cv::waitKey(16) == 27) { 129 | exit(0); 130 | } 131 | } 132 | return frontend->ObserveImage(left_image, right_image, image_time); 133 | } 134 | 135 | void OdometryCallback(const nav_msgs::Odometry& msg, 136 | Frontend* frontend) { 137 | if (FLAGS_v > 1) { 138 | printf("Odometry t=%f\n", msg.header.stamp.toSec()); 139 | } 140 | const Vector3f odom_loc(msg.pose.pose.position.x, 141 | msg.pose.pose.position.y, 142 | msg.pose.pose.position.z); 143 | const Quaternionf odom_angle(msg.pose.pose.orientation.w, 144 | msg.pose.pose.orientation.x, 145 | msg.pose.pose.orientation.y, 146 | msg.pose.pose.orientation.z); 147 | frontend->ObserveOdometry(odom_loc, odom_angle, msg.header.stamp.toSec()); 148 | } 149 | 150 | using std::isfinite; 151 | bool IsFinite(const Vector3f& p) { 152 | return isfinite(p.x()) && isfinite(p.y()) && isfinite(p.z()); 153 | } 154 | 155 | void AddFeaturePoints(const slam::FrontendConfig& config, 156 | const SLAMProblem& problem, 157 | Marker* marker_ptr) { 158 | const Affine3f cam_to_robot = config.left_cam_to_robot; 159 | for (const SLAMNode& node : problem.nodes) { 160 | const Affine3f robot_to_world = node.pose.RobotToWorldTf(); 161 | for (const slam_types::VisionFeature& f : node.features) { 162 | if (IsFinite(f.point3d)) { 163 | if (f.point3d.z() > 0.1 && 164 | f.point3d.norm() > 0.5 && 165 | f.point3d.norm() < 20.0) { 166 | AddPoint(robot_to_world * cam_to_robot * f.point3d, 167 | Color4f(1, 1, 1, 0.2), 168 | marker_ptr); 169 | } 170 | } 171 | } 172 | } 173 | } 174 | 175 | void AddPoseGraph(const SLAMProblem& problem, 176 | Marker* nodes_marker, 177 | Marker* vision_marker, 178 | Marker* odom_marker) { 179 | for (const SLAMNode& node : problem.nodes) { 180 | AddPoint(node.pose.loc, Color4f::kRed, nodes_marker); 181 | } 182 | for (const OdometryFactor& factor : problem.odometry_factors) { 183 | const Vector3f& loc1 = problem.nodes[factor.pose_i].pose.loc; 184 | const Vector3f& loc2 = problem.nodes[factor.pose_j].pose.loc; 185 | AddLine(loc1, loc2, Color4f::kGreen, odom_marker); 186 | } 187 | for (const VisionFactor& factor : problem.vision_factors) { 188 | const Vector3f& loc1 = problem.nodes[factor.pose_idx_initial].pose.loc; 189 | const Vector3f& loc2 = problem.nodes[factor.pose_idx_current].pose.loc; 190 | AddLine(loc1, loc2, Color4f::kBlue, vision_marker); 191 | } 192 | } 193 | 194 | void PublishVisualization(const slam::FrontendConfig& config, 195 | const SLAMProblem& problem, 196 | ros::Publisher* graph_publisher, 197 | ros::Publisher* point_cloud_publisher) { 198 | Marker nodes_marker; 199 | Marker odom_marker; 200 | Marker vision_marker; 201 | Marker vision_points_marker; 202 | InitializeMarker( 203 | Marker::POINTS, Color4f::kRed, 0.05, 0.1, 0, &nodes_marker); 204 | InitializeMarker( 205 | Marker::LINE_LIST, Color4f::kGreen, 0.02, 0, 0, &odom_marker); 206 | InitializeMarker( 207 | Marker::LINE_LIST, Color4f::kBlue, 0.01, 0, 0, &vision_marker); 208 | InitializeMarker( 209 | Marker::POINTS, Color4f::kWhite, 0.025, 0.025, 0.025, 210 | &vision_points_marker); 211 | nodes_marker.id = 0; 212 | odom_marker.id = 1; 213 | vision_marker.id = 2; 214 | vision_points_marker.id = 3; 215 | 216 | AddFeaturePoints(config, problem, &vision_points_marker); 217 | AddPoseGraph(problem, &nodes_marker, &vision_marker, &odom_marker); 218 | 219 | MarkerArray markers; 220 | markers.markers.push_back(nodes_marker); 221 | markers.markers.push_back(odom_marker); 222 | markers.markers.push_back(vision_marker); 223 | graph_publisher->publish(markers); 224 | point_cloud_publisher->publish(vision_points_marker); 225 | } 226 | 227 | // Comparator used to determine left and right frame pairs. 228 | class SeqCompare { 229 | public: 230 | bool operator()(sensor_msgs::CompressedImage a, 231 | sensor_msgs::CompressedImage b) { 232 | return a.header.seq < b.header.seq; 233 | } 234 | }; 235 | 236 | void ProcessBagfile(const char* filename, ros::NodeHandle* n) { 237 | rosbag::Bag bag; 238 | cv_bridge::CvImage img_tranform; 239 | try { 240 | bag.open(filename, rosbag::bagmode::Read); 241 | } catch(rosbag::BagException& exception) { 242 | printf("Unable to read %s, reason:\n %s\n", filename, exception.what()); 243 | return; 244 | } 245 | printf("Processing %s\n", filename); 246 | vector topics; 247 | topics.push_back(FLAGS_left_image_topic.c_str()); 248 | topics.push_back(FLAGS_right_image_topic.c_str()); 249 | topics.push_back(FLAGS_odom_topic.c_str()); 250 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 251 | slam::Frontend slam_frontend(""); 252 | ros::Publisher gui_publisher = n->advertise( 253 | "slam_frontend/pose_graph", 1); 254 | ros::Publisher debug_image_publisher = n->advertise( 255 | "slam_frontend/debug_image", 1); 256 | ros::Publisher debug_stereo_image_publisher = 257 | n->advertise("slam_frontend/debug_stereo_image", 1); 258 | ros::Publisher point_cloud_publisher = 259 | n->advertise("slam_frontend/points", 1); 260 | double bag_t_start = -1; 261 | bool max_poses_processed = false; 262 | std::pair 263 | curr_stereo_image_pair; 264 | // Iterate for every message. 265 | std::priority_queue, 267 | SeqCompare> left_queue; 268 | std::priority_queue, 270 | SeqCompare> right_queue; 271 | for (rosbag::View::iterator it = view.begin(); 272 | ros::ok() && it != view.end() && !max_poses_processed; 273 | ++it) { 274 | const rosbag::MessageInstance& message = *it; 275 | bool new_pose_added = false; 276 | if (bag_t_start < 0.0) { 277 | bag_t_start = message.getTime().toSec(); 278 | } 279 | { 280 | sensor_msgs::CompressedImagePtr image_msg = 281 | message.instantiate(); 282 | if (image_msg != NULL) { 283 | const bool is_left_camera = 284 | message.getTopic() == FLAGS_left_image_topic; 285 | new_pose_added = false; 286 | if (is_left_camera) { 287 | curr_stereo_image_pair.first = *image_msg; 288 | } else { 289 | curr_stereo_image_pair.second = *image_msg; 290 | CHECK_EQ(curr_stereo_image_pair.first.header.stamp.toNSec(), 291 | curr_stereo_image_pair.second.header.stamp.toNSec()); 292 | new_pose_added = CompressedImageCallback(curr_stereo_image_pair, 293 | &slam_frontend); 294 | } 295 | if (new_pose_added) { 296 | if (FLAGS_max_poses > 0 && 297 | slam_frontend.GetNumPoses() > FLAGS_max_poses) { 298 | max_poses_processed = true; 299 | } 300 | static std_msgs::Header debug_image_header; 301 | debug_image_header.seq++; 302 | debug_image_header.stamp = ros::Time::now(); 303 | img_tranform.image = slam_frontend.GetLastDebugImage(); 304 | img_tranform.encoding = sensor_msgs::image_encodings::RGB8; 305 | debug_image_publisher.publish(img_tranform.toImageMsg()); 306 | img_tranform.image = slam_frontend.GetLastDebugStereoImage(); 307 | img_tranform.encoding = sensor_msgs::image_encodings::RGB8; 308 | debug_stereo_image_publisher.publish(img_tranform.toImageMsg()); 309 | } 310 | } 311 | } 312 | { 313 | nav_msgs::OdometryPtr odom_msg = 314 | message.instantiate(); 315 | if (odom_msg != NULL) { 316 | OdometryCallback(*odom_msg, &slam_frontend); 317 | } 318 | } 319 | if (new_pose_added) { 320 | SLAMProblem problem; 321 | slam_frontend.GetSLAMProblem(&problem); 322 | PublishVisualization(slam_frontend.GetConfig(), 323 | problem, 324 | &gui_publisher, 325 | &point_cloud_publisher); 326 | ros::spinOnce(); 327 | } 328 | } 329 | printf("Done processing bag file.\n"); 330 | // Publish slam data 331 | rosbag::Bag output_bag; 332 | try { 333 | output_bag.open(FLAGS_output.c_str(), rosbag::bagmode::Write); 334 | } catch(rosbag::BagException& exception) { 335 | printf("Unable to open %s, reason:\n %s\n", 336 | FLAGS_output.c_str(), 337 | exception.what()); 338 | return; 339 | } 340 | 341 | slam_types::CameraExtrinsics a; 342 | const Affine3f extrinsics = slam_frontend.GetConfig().left_cam_to_robot; 343 | const Vector3f rT = extrinsics.translation(); 344 | const AngleAxisf rR(extrinsics.rotation()); 345 | 346 | for (int i = 0; i < 3; ++i) { 347 | a.translation[i] = rT[i]; 348 | if (rR.angle() > 1e-8) { 349 | a.rotation[i] = rR.axis().normalized()[i] * rR.angle(); 350 | } else { 351 | a.rotation[i] = 0; 352 | } 353 | } 354 | output_bag.write( 355 | "extrinsics", 356 | ros::Time::now(), 357 | ExtrinsicsToRos(a)); 358 | vision_slam_frontend::CameraIntrinsics k; 359 | k.fx = slam_frontend.GetConfig().intrinsics_left.fx; 360 | k.cx = slam_frontend.GetConfig().intrinsics_left.cx; 361 | k.fy = slam_frontend.GetConfig().intrinsics_left.fy; 362 | k.cy = slam_frontend.GetConfig().intrinsics_left.cy; 363 | 364 | output_bag.write( 365 | "intrinsics", 366 | ros::Time::now(), 367 | k); 368 | 369 | SLAMProblem problem; 370 | slam_frontend.GetSLAMProblem(&problem); 371 | output_bag.write( 372 | "slam_problem", 373 | ros::Time::now(), 374 | SLAMProblemToRos(problem)); 375 | printf("Saved SLAM problem with %d nodes, %d odometry factors, " 376 | "%d vision factors (%.2f/pose avg)\n", 377 | static_cast(problem.nodes.size()), 378 | static_cast(problem.odometry_factors.size()), 379 | static_cast(problem.vision_factors.size()), 380 | static_cast(problem.vision_factors.size()) / 381 | static_cast((problem.nodes.size() - 1))); 382 | if (FLAGS_save_debug) { 383 | { 384 | std::vector debug_images = slam_frontend.getDebugImages(); 385 | uint64_t count = 0; 386 | for (auto im : debug_images) { 387 | std_msgs::Header h; 388 | h.seq = count++; 389 | h.stamp = ros::Time::now(); 390 | img_tranform.image = im; 391 | img_tranform.encoding = sensor_msgs::image_encodings::RGB8; 392 | output_bag.write("slam_debug_images", 393 | ros::Time::now(), 394 | img_tranform.toImageMsg()); 395 | } 396 | } 397 | { 398 | std::vector debug_stereo_images = 399 | slam_frontend.getDebugStereoImages(); 400 | uint64_t count = 0; 401 | for (auto im : debug_stereo_images) { 402 | printf("%d\n", im.flags); 403 | std_msgs::Header h; 404 | h.seq = count++; 405 | h.stamp = ros::Time::now(); 406 | img_tranform.image = im; 407 | img_tranform.encoding = sensor_msgs::image_encodings::RGB8; 408 | output_bag.write("slam_debug_stereo_images", 409 | ros::Time::now(), 410 | img_tranform.toImageMsg()); 411 | } 412 | } 413 | } 414 | output_bag.close(); 415 | } 416 | 417 | void SignalHandler(int signum) { 418 | printf("Exiting with signal %d\n", signum); 419 | exit(0); 420 | } 421 | 422 | int main(int argc, char** argv) { 423 | google::InitGoogleLogging(*argv); 424 | google::ParseCommandLineFlags(&argc, &argv, false); 425 | if (FLAGS_input == "") { 426 | fprintf(stderr, "ERROR: Must specify input file!\n"); 427 | return 1; 428 | } 429 | if (FLAGS_output == "") { 430 | fprintf(stderr, "ERROR: Must specify output file!\n"); 431 | return 1; 432 | } 433 | // Initialize ROS. 434 | ros::init(argc, argv, "slam_frontend"); 435 | ros::NodeHandle n; 436 | signal(SIGINT, SignalHandler); 437 | ProcessBagfile(FLAGS_input.c_str(), &n); 438 | return 0; 439 | } 440 | -------------------------------------------------------------------------------- /src/slam_frontend.cc: -------------------------------------------------------------------------------- 1 | //======================================================================== 2 | // This software is free: you can redistribute it and/or modify 3 | // it under the terms of the GNU Lesser General Public License Version 3, 4 | // as published by the Free Software Foundation. 5 | // 6 | // This software is distributed in the hope that it will be useful, 7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | // GNU Lesser General Public License for more details. 10 | // 11 | // You should have received a copy of the GNU Lesser General Public License 12 | // Version 3 in the file COPYING that came with this distribution. 13 | // If not, see . 14 | //======================================================================== 15 | /*! 16 | \file slam_frontend.cc 17 | \brief A vision SLAM frontend 18 | \author Joydeep Biswas, (C) 2018 19 | */ 20 | //======================================================================== 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "eigen3/Eigen/Dense" 29 | #include "eigen3/Eigen/Geometry" 30 | #include "opencv2/opencv.hpp" 31 | #include "opencv2/features2d.hpp" 32 | #include "opencv2/imgproc.hpp" 33 | #ifdef USE_XFEATURES 34 | #include "opencv2/xfeatures2d.hpp" 35 | #endif 36 | #include "glog/logging.h" 37 | #include "opencv2/core/eigen.hpp" 38 | 39 | #include "slam_frontend.h" 40 | 41 | using slam_types::RobotPose; 42 | using slam_types::SLAMNode; 43 | using slam_types::SLAMProblem; 44 | using slam_types::OdometryFactor; 45 | using slam_types::VisionFactor; 46 | using slam_types::FeatureMatch; 47 | using slam_types::VisionFeature; 48 | using std::cout; 49 | using std::endl; 50 | using std::string; 51 | using std::vector; 52 | using Eigen::Quaternionf; 53 | using Eigen::Vector2f; 54 | using Eigen::Vector3f; 55 | using Eigen::Matrix3f; 56 | using Eigen::Matrix; 57 | 58 | /* --- Frontend Implementation Code --- */ 59 | 60 | namespace { 61 | template 62 | Eigen::Matrix OpenCVToEigen(const cv::Point_& p) { 63 | return Eigen::Matrix(p.x, p.y); 64 | } 65 | 66 | template 67 | cv::Point_ EigenToOpenCV(const Eigen::Matrix& p) { 68 | return cv::Point_(p.x(), p.y()); 69 | } 70 | 71 | } // namespace 72 | namespace slam { 73 | 74 | /* Debug Image Generators */ 75 | cv::Mat CreateStereoDebugImage(const Frame& frame_left, 76 | const Frame& frame_right, 77 | const VisionFactor& matches) { 78 | int left_cols = frame_left.debug_image_.cols; 79 | cv::Mat return_image; 80 | cv::hconcat(frame_left.debug_image_, frame_right.debug_image_, return_image); 81 | // Convert debug image to RGB so we can draw other colors on it. 82 | cv::cvtColor(return_image, return_image, cv::COLOR_GRAY2BGR); 83 | // Draw all the stereo matches on the final image. 84 | for (FeatureMatch match : matches.feature_matches) { 85 | cv::Point2f left_point = 86 | frame_left.keypoints_[match.feature_idx_current].pt; 87 | cv::circle(return_image, left_point, 5, CV_RGB(255, 0, 0)); 88 | cv::Point2f right_point = 89 | frame_right.keypoints_[match.feature_idx_initial].pt; 90 | right_point.x += left_cols; 91 | cv::circle(return_image, right_point, 5, CV_RGB(255, 0, 0)); 92 | cv::line(return_image, 93 | left_point, 94 | right_point, 95 | cv::Scalar(rand() % 255, rand() % 255, rand() % 255)); 96 | } 97 | return return_image; 98 | } 99 | 100 | cv::Mat CreateMatchDebugImage(const Frame& frame_initial, 101 | const Frame& frame_current, 102 | const VisionFactor& corr) { 103 | cv::Mat return_image = frame_current.debug_image_.clone(); 104 | cv::cvtColor(return_image, return_image, cv::COLOR_GRAY2RGB); 105 | for (const slam_types::FeatureMatch c : corr.feature_matches) { 106 | cv::circle(return_image, 107 | frame_initial.keypoints_[c.feature_idx_initial].pt, 108 | 5, CV_RGB(255, 0, 0)); 109 | cv::line(return_image, 110 | frame_initial.keypoints_[c.feature_idx_initial].pt, 111 | frame_current.keypoints_[c.feature_idx_current].pt, 112 | CV_RGB(0, 255, 0)); 113 | } 114 | return return_image; 115 | } 116 | 117 | void Frontend::Calculate3DPoints(Frame* left_frame, 118 | Frame* right_frame, 119 | vector* points) { 120 | // Convert keypoints into array of points. 121 | std::vector left_points; 122 | std::vector right_points; 123 | // The reason we pass in right frame first is because we don't want to modify 124 | // the left frames is_initial_ book-keeping. 125 | // This means right will be the 'initial' and the left_frame 126 | // will be the 'current' frame. 127 | VisionFactor* matches; 128 | // Assure that every point has a match. 129 | float best_percent = config_.best_percent_; 130 | config_.best_percent_ = 1.0; 131 | matches = GetFeatureMatches(right_frame, left_frame); 132 | config_.best_percent_ = best_percent; 133 | if (matches->feature_matches.size() == 0) { 134 | return; 135 | } 136 | for (FeatureMatch match : matches->feature_matches) { 137 | const auto& left_pt = left_frame->keypoints_[match.feature_idx_current].pt; 138 | const auto& right_pt = 139 | right_frame->keypoints_[match.feature_idx_initial].pt; 140 | // if (fabs(left_pt.y - right_pt.y) > 5) continue; 141 | right_points.push_back(right_pt); 142 | left_points.push_back(left_pt); 143 | if (false) { 144 | printf("[%6.1f %6.1f] [%6.1f %6.1f]\n", 145 | left_pt.x, 146 | left_pt.y, 147 | right_pt.x, 148 | right_pt.y); 149 | } 150 | } 151 | cv::Mat triangulated_points; 152 | cv::triangulatePoints(config_.projection_left, 153 | config_.projection_right, 154 | left_points, 155 | right_points, 156 | triangulated_points); 157 | // Make sure all keypoints are matched to something. 158 | CHECK_EQ(triangulated_points.cols, matches->feature_matches.size()); 159 | for (int64_t c = 0; c < triangulated_points.cols; ++c) { 160 | cv::Mat col = triangulated_points.col(c); 161 | points->push_back( 162 | Vector3f(col.at(0, 0), 163 | col.at(1, 0), 164 | col.at(2, 0)) /col.at(3, 0)); 165 | } 166 | // Generate Debug Images if needed 167 | if (config_.debug_images_) { 168 | debug_stereo_images_.push_back(CreateStereoDebugImage(*left_frame, 169 | *right_frame, 170 | *matches)); 171 | } 172 | free(matches); 173 | } 174 | 175 | bool Frontend::OdomCheck() { 176 | if (!odom_initialized_) return false; 177 | if ((prev_odom_translation_ - odom_translation_).norm() > 178 | config_.min_odom_translation) { 179 | return true; 180 | } 181 | if (prev_odom_rotation_.angularDistance(odom_rotation_) > 182 | config_.min_odom_rotation) { 183 | return true; 184 | } 185 | return false; 186 | } 187 | 188 | Frontend::Frontend(const string& config_path) : 189 | odom_initialized_(false), 190 | curr_frame_ID_(0) { 191 | fast_feature_detector_ = cv::FastFeatureDetector::create(10, true); 192 | switch (config_.descriptor_extract_type_) { 193 | case FrontendConfig::DescriptorExtractorType::AKAZE: { 194 | descriptor_extractor_ = cv::AKAZE::create(cv::AKAZE::DESCRIPTOR_MLDB, 195 | 0, 196 | 3, 197 | 0.0001f, 198 | 10, 199 | 5, 200 | cv::KAZE::DIFF_PM_G2); 201 | config_.bf_matcher_param_ = cv::NORM_HAMMING; 202 | break; 203 | } 204 | case FrontendConfig::DescriptorExtractorType::ORB: { 205 | descriptor_extractor_ = cv::ORB::create(10000, 206 | 1.04f, 207 | 50, 208 | 31, 209 | 0, 210 | 2, 211 | cv::ORB::HARRIS_SCORE, 212 | 31, 213 | 20); 214 | config_.bf_matcher_param_ = cv::NORM_HAMMING; 215 | break; 216 | } 217 | case FrontendConfig::DescriptorExtractorType::BRISK: { 218 | descriptor_extractor_ = cv::BRISK::create(20, 7, 1.1f); 219 | config_.bf_matcher_param_ = cv::NORM_HAMMING; 220 | break; 221 | } 222 | #ifdef USE_XFEATURES 223 | case FrontendConfig::DescriptorExtractorType::SURF: { 224 | descriptor_extractor_ = cv::xfeatures2d::SURF::create(); 225 | config_.bf_matcher_param_ = cv::NORM_L2; 226 | break; 227 | } 228 | case FrontendConfig::DescriptorExtractorType::SIFT: { 229 | descriptor_extractor_ = cv::xfeatures2d::SIFT::create(); 230 | config_.bf_matcher_param_ = cv::NORM_L2; 231 | break; 232 | } 233 | case FrontendConfig::DescriptorExtractorType::FREAK: { 234 | descriptor_extractor_ = cv::xfeatures2d::FREAK::create(false, 235 | true, 236 | 40.0f, 237 | 20); 238 | config_.bf_matcher_param_ = cv::NORM_HAMMING; 239 | break; 240 | } 241 | #endif 242 | default: { 243 | LOG(ERROR) << "Could not recognize descriptor extractor option."; 244 | exit(1); 245 | } 246 | } 247 | matcher_ = cv::BFMatcher::create(config_.bf_matcher_param_); 248 | } 249 | 250 | void Frontend::ObserveOdometry(const Vector3f& translation, 251 | const Quaternionf& rotation, 252 | double timestamp) { 253 | if (!odom_initialized_) { 254 | init_odom_rotation_ = rotation; 255 | init_odom_translation_ = translation; 256 | prev_odom_rotation_ = odom_rotation_; 257 | prev_odom_translation_ = odom_translation_; 258 | odom_initialized_ = true; 259 | } 260 | odom_translation_ = translation; 261 | odom_rotation_ = rotation; 262 | odom_timestamp_ = timestamp; 263 | } 264 | 265 | 266 | void Frontend::ExtractFeatures(cv::Mat image, Frame* frame) { 267 | vector frame_keypoints; 268 | cv::Mat frame_descriptors; 269 | if (config_.descriptor_extract_type_ == 270 | FrontendConfig::DescriptorExtractorType::FREAK) { 271 | fast_feature_detector_->detect(image, frame_keypoints); 272 | descriptor_extractor_->compute(image, frame_keypoints, frame_descriptors); 273 | } else { 274 | descriptor_extractor_->detectAndCompute(image, 275 | cv::noArray(), 276 | frame_keypoints, 277 | frame_descriptors); 278 | } 279 | *frame = Frame(frame_keypoints, frame_descriptors, curr_frame_ID_); 280 | } 281 | 282 | VisionFactor* Frontend::GetFeatureMatches(Frame* past_frame_ptr, 283 | Frame* curr_frame_ptr) { 284 | Frame& past_frame = *past_frame_ptr; 285 | Frame& curr_frame = *curr_frame_ptr; 286 | vector pairs; 287 | vector matches = 288 | GetMatches(past_frame, curr_frame, config_.nn_match_ratio_); 289 | std::sort(matches.begin(), matches.end()); 290 | const int num_good_matches = matches.size() * config_.best_percent_; 291 | matches.erase(matches.begin() + num_good_matches, matches.end()); 292 | // Restructure matches, add all keypoints to new list. 293 | for (auto match : matches) { 294 | // Add it to vision factor. 295 | pairs.push_back(FeatureMatch(match.queryIdx, 296 | match.trainIdx)); 297 | // Check if this is the first time we are seeing this match. 298 | // If it is not, then mark it where it was first seen. 299 | if (curr_frame.is_initial_[match.trainIdx]) { 300 | curr_frame.is_initial_[match.trainIdx] = false; 301 | curr_frame.initial_ids_[match.trainIdx] = 302 | (past_frame.is_initial_[match.queryIdx])? 303 | past_frame.frame_ID_ : 304 | past_frame.initial_ids_[match.queryIdx]; 305 | } 306 | } 307 | // printf("%lu\n", pairs.size()); 308 | return new VisionFactor(past_frame.frame_ID_, curr_frame.frame_ID_, pairs); 309 | } 310 | 311 | void Frontend::AddOdometryFactor() { 312 | const Vector3f translation = 313 | prev_odom_rotation_.inverse() * (odom_translation_ - 314 | prev_odom_translation_); 315 | const Quaternionf rotation(odom_rotation_ * prev_odom_rotation_.inverse()); 316 | odometry_factors_.push_back(OdometryFactor( 317 | curr_frame_ID_ - 1, 318 | curr_frame_ID_, 319 | translation, 320 | rotation)); 321 | } 322 | 323 | void Frontend::UndistortFeaturePoints(vector* features_ptr) { 324 | vector& features = *features_ptr; 325 | if (features.size() == 0) { 326 | return; 327 | } 328 | const Vector2f p0(config_.intrinsics_left.cx, config_.intrinsics_left.cy); 329 | vector distorted_pts; 330 | for (size_t i = 0; i < features.size(); ++i) { 331 | distorted_pts.push_back(EigenToOpenCV(features[i].pixel)); 332 | } 333 | vector undistorted_pts; 334 | cv::undistortPoints(distorted_pts, 335 | undistorted_pts, 336 | config_.camera_matrix_left, 337 | config_.distortion_coeffs_left, 338 | cv::noArray(), 339 | config_.camera_matrix_left); 340 | CHECK_EQ(distorted_pts.size(), undistorted_pts.size()); 341 | for (size_t i = 0; i < features.size(); ++i) { 342 | if (false) { 343 | printf("[%.3f %.3f] -> [%.3f %.3f]\n", 344 | distorted_pts[i].x, 345 | distorted_pts[i].y, 346 | undistorted_pts[i].x, 347 | undistorted_pts[i].y); 348 | } 349 | features[i].pixel = OpenCVToEigen(undistorted_pts[i]); 350 | } 351 | } 352 | 353 | static float stereo_ambig_constraint = 10000; 354 | void Frontend::RemoveAmbigStereo(Frame* left, 355 | Frame* right, 356 | const std::vector stereo_matches) { 357 | // Get the left and right points. 358 | std::vector left_points; 359 | std::vector right_points; 360 | for (uint64 m = 0; m < stereo_matches.size(); m++) { 361 | cv::DMatch match = stereo_matches[m]; 362 | left_points.push_back(left->keypoints_[match.queryIdx].pt); 363 | right_points.push_back(right->keypoints_[match.trainIdx].pt); 364 | } 365 | // cv::Mat fund = cv::findFundamentalMat(left_points, right_points); 366 | // Convert to Eigen format 367 | std::vector left_keypoints, right_keypoints; 368 | cv::Mat left_descs, right_descs; 369 | float avg_constraint = 0.0f; 370 | for (uint64 m = 0; m < stereo_matches.size(); m++) { 371 | // Transform into homogenous coordinates 372 | Eigen::Matrix left_ph; 373 | left_ph[0] = left_points[m].x; 374 | left_ph[1] = left_points[m].y; 375 | left_ph[2] = 1.0f; 376 | Eigen::Matrix right_ph; 377 | right_ph[0] = right_points[m].x; 378 | right_ph[1] = right_points[m].y; 379 | right_ph[2] = 1.0f; 380 | float constraint = 381 | (left_ph.transpose() * config_.fundamental * right_ph).norm(); 382 | avg_constraint += constraint; 383 | if (constraint <= stereo_ambig_constraint) { 384 | // This set of points is considered non-ambigious, keep it. 385 | cv::DMatch match = stereo_matches[m]; 386 | left_keypoints.push_back(left->keypoints_[match.queryIdx]); 387 | right_keypoints.push_back(right->keypoints_[match.trainIdx]); 388 | left_descs.push_back(left->descriptors_.row(match.queryIdx)); 389 | right_descs.push_back(right->descriptors_.row(match.trainIdx)); 390 | } 391 | } 392 | float padding_from_average = 2.0f; 393 | stereo_ambig_constraint = 394 | avg_constraint / stereo_matches.size() + padding_from_average; 395 | // Set the left and right data to the update non-ambigious data. 396 | *left = Frame(left_keypoints, left_descs, left->frame_ID_); 397 | *right = Frame(right_keypoints, right_descs, right->frame_ID_); 398 | } 399 | 400 | bool Frontend::ObserveImage(const cv::Mat& left_image, 401 | const cv::Mat& right_image, 402 | double time) { 403 | // Check from the odometry if its time to run SLAM 404 | if (!OdomCheck()) { 405 | return false; 406 | } 407 | if (FLAGS_v > 2) { 408 | printf("Observing Frame at %d\n", static_cast(frame_list_.size())); 409 | } 410 | Frame curr_frame, right_temp_frame; 411 | ExtractFeatures(left_image, &curr_frame); 412 | ExtractFeatures(right_image, &right_temp_frame); 413 | // Remove ambigious stereo matching points from left's and right's points. 414 | std::vector stereo_matches = GetMatches(curr_frame, 415 | right_temp_frame, 416 | config_.nn_match_ratio_); 417 | RemoveAmbigStereo(&curr_frame, &right_temp_frame, stereo_matches); 418 | // If we are running a debug version, attach image to frame. 419 | if (config_.debug_images_) { 420 | curr_frame.debug_image_ = left_image; 421 | right_temp_frame.debug_image_ = right_image; 422 | } 423 | // Keep track of the points that are original to this frame. 424 | for (Frame& past_frame : frame_list_) { 425 | VisionFactor* matches; 426 | matches = GetFeatureMatches(&past_frame, &curr_frame); 427 | // TODO(Jack): verify that this conditional check does not mess up 428 | // book-keping. 429 | // if (matches.feature_matches.size() > config_.min_vision_matches) { 430 | // vision_factors_.push_back(matches); 431 | // } 432 | vision_factors_.push_back(*matches); 433 | free(matches); 434 | } 435 | // Calculate the depths of the points. 436 | vector points; 437 | Calculate3DPoints(&curr_frame, &right_temp_frame, &points); 438 | vector features; 439 | for (uint64_t i = 0; i < curr_frame.keypoints_.size(); i++) { 440 | features.push_back(VisionFeature( 441 | i, OpenCVToEigen(curr_frame.keypoints_[i].pt), points[i])); 442 | } 443 | UndistortFeaturePoints(&features); 444 | const Vector3f loc = init_odom_rotation_.inverse() * 445 | (odom_translation_ - init_odom_translation_); 446 | const Quaternionf angle = odom_rotation_ * init_odom_rotation_.inverse(); 447 | nodes_.push_back(SLAMNode(curr_frame_ID_, 448 | odom_timestamp_, 449 | RobotPose(loc, 450 | angle), 451 | features)); 452 | if (curr_frame_ID_ > 0) { 453 | AddOdometryFactor(); 454 | } 455 | prev_odom_rotation_ = odom_rotation_; 456 | prev_odom_translation_ = odom_translation_; 457 | curr_frame_ID_++; 458 | if (config_.debug_images_ && 459 | !frame_list_.empty() && 460 | !vision_factors_.empty()) { 461 | const slam_types::VisionFactor factor = vision_factors_.back(); 462 | const Frame initial_frame = frame_list_.back(); 463 | CHECK_EQ(factor.pose_idx_initial, initial_frame.frame_ID_); 464 | debug_images_.push_back( 465 | CreateMatchDebugImage(initial_frame, curr_frame, factor)); 466 | } 467 | if (frame_list_.size() >= config_.frame_life_) { 468 | frame_list_.erase(frame_list_.begin()); 469 | } 470 | frame_list_.push_back(curr_frame); 471 | return true; 472 | } 473 | 474 | vector Frontend::getDebugImages() { 475 | return debug_images_; 476 | } 477 | 478 | vector Frontend::getDebugStereoImages() { 479 | return debug_stereo_images_; 480 | } 481 | 482 | 483 | cv::Mat Frontend::GetLastDebugImage() { 484 | if (debug_images_.size() == 0) { 485 | return cv::Mat(); 486 | } 487 | return debug_images_.back(); 488 | } 489 | 490 | cv::Mat Frontend::GetLastDebugStereoImage() { 491 | if (debug_stereo_images_.size() == 0) { 492 | return cv::Mat(); 493 | } 494 | return debug_stereo_images_.back(); 495 | } 496 | 497 | 498 | void Frontend::GetSLAMProblem(SLAMProblem* problem) const { 499 | *problem = slam_types::SLAMProblem( 500 | nodes_, 501 | vision_factors_, 502 | odometry_factors_); 503 | } 504 | 505 | int Frontend::GetNumPoses() { 506 | return nodes_.size(); 507 | } 508 | 509 | /* --- Frame Implementation Code --- */ 510 | 511 | Frame::Frame(const vector& keypoints, 512 | const cv::Mat& descriptors, 513 | uint64_t frame_ID) { 514 | keypoints_ = keypoints; 515 | descriptors_ = descriptors; 516 | frame_ID_ = frame_ID; 517 | is_initial_ = std::vector(keypoints_.size(), true); 518 | initial_ids_ = std::vector(keypoints_.size(), -1); 519 | } 520 | 521 | vector Frontend::GetMatches(const Frame& frame_query, 522 | const Frame& frame_train, 523 | double nn_match_ratio) { 524 | vector> matches; 525 | matcher_->knnMatch(frame_query.descriptors_, 526 | frame_train.descriptors_, 527 | matches, 2); 528 | vector best_matches; 529 | for (size_t i = 0; i < matches.size(); i++) { 530 | cv::DMatch first = matches[i][0]; 531 | float dist1 = matches[i][0].distance; 532 | float dist2 = matches[i][1].distance; 533 | if (dist1 < nn_match_ratio * dist2) { 534 | best_matches.push_back(first); 535 | } 536 | } 537 | return best_matches; 538 | } 539 | 540 | /* --- Config Implementation Code --- */ 541 | 542 | Matrix3f CameraMatrix(const CameraIntrinsics& I) { 543 | Matrix3f M; 544 | M << I.fx, 0, I.cx, 545 | 0, I.fy, I.cy, 546 | 0, 0, 1; 547 | return M; 548 | } 549 | 550 | FrontendConfig::FrontendConfig() { 551 | // Load Default values 552 | debug_images_ = true; 553 | descriptor_extract_type_ = FrontendConfig::DescriptorExtractorType::AKAZE; 554 | best_percent_ = 0.3f; 555 | nn_match_ratio_ = 0.6f; 556 | frame_life_ = 10; 557 | min_odom_rotation = 10.0 / 180.0 * M_PI; 558 | min_odom_translation = 0.2; 559 | min_vision_matches = 10; 560 | 561 | // Taken from Campus-Jackal repo: 562 | // https://github.com/umass-amrl/Campus-Jackal/blob/master/hardware/ 563 | // calibration/PointGreyCalibration/6_Aug_24_18/jpp/ 564 | // pointgrey_calib_6_Aug_24_18.yaml 565 | intrinsics_left.fx = 527.873518; 566 | intrinsics_left.cx = 482.823413; 567 | intrinsics_left.fy = 527.276819; 568 | intrinsics_left.cy = 298.033945; 569 | intrinsics_left.k1 = -0.153137; 570 | intrinsics_left.k2 = 0.075666; 571 | intrinsics_left.p1 = -0.000227; 572 | intrinsics_left.p2 = -0.000320; 573 | intrinsics_left.k3 = 0; 574 | 575 | intrinsics_right.fx = 530.158021; 576 | intrinsics_right.cx = 475.540633; 577 | intrinsics_right.fy = 529.682234; 578 | intrinsics_right.cy = 299.995465; 579 | intrinsics_right.k1 = -0.156833; 580 | intrinsics_right.k2 = 0.081841; 581 | intrinsics_right.p1 = -0.000779; 582 | intrinsics_right.p2 = -0.000356; 583 | intrinsics_right.k3 = -0.000779; 584 | 585 | camera_matrix_left = (cv::Mat_(3, 3) << 586 | intrinsics_left.fx, 0, intrinsics_left.cx, 587 | 0, intrinsics_left.fy, intrinsics_left.cy, 588 | 0, 0, 1); 589 | 590 | camera_matrix_right = (cv::Mat_(3, 3) << 591 | intrinsics_right.fx, 0, intrinsics_right.cx, 592 | 0, intrinsics_right.fy, intrinsics_right.cy, 593 | 0, 0, 1); 594 | 595 | const Matrix3f K_left = CameraMatrix(intrinsics_left); 596 | Matrix A_left; 597 | A_left << 1, 0, 0, 0, 598 | 0, 1, 0, 0, 599 | 0, 0, 1, 0; 600 | const Matrix P_left = K_left * A_left; 601 | 602 | const Matrix3f K_right = CameraMatrix(intrinsics_right); 603 | Matrix A_right; 604 | A_right << 605 | 0.999593617649873, 0.021411909431148, -0.018818333830411, 606 | -0.131707087331978, 607 | -0.021140534893290, 0.999671312094879, 0.014503294761121, 608 | 0.003232397463343, 609 | 0.019122691705565, -0.014099571235136, 0.999717722536176, 610 | -0.001146108483477; 611 | const Matrix P_right = K_right * A_right; 612 | 613 | const Vector3f XT(-0.01, 0.06, 0.5299999713897705); 614 | Matrix3f RT; 615 | RT << 0.009916590468, -0.2835522866, 0.9589055021, 616 | -0.9998698619, -0.01501486552, 0.005900269087, 617 | 0.01272480238, -0.9588392225, -0.2836642819; 618 | left_cam_to_robot = Eigen::Translation3f(XT) * RT; 619 | projection_left = cv::Mat(3, 4, CV_32F); 620 | projection_right = cv::Mat(3, 4, CV_32F); 621 | cv::eigen2cv(P_left, projection_left); 622 | cv::eigen2cv(P_right, projection_right); 623 | distortion_coeffs_left = (cv::Mat_(5, 1) << 624 | intrinsics_left.k1, 625 | intrinsics_left.k2, 626 | intrinsics_left.p1, 627 | intrinsics_left.p2, 628 | intrinsics_left.k3); 629 | distortion_coeffs_right = (cv::Mat_(5, 1) << 630 | intrinsics_right.k1, 631 | intrinsics_right.k2, 632 | intrinsics_right.p1, 633 | intrinsics_right.p2, 634 | intrinsics_right.k3); 635 | Matrix3f ecam_left, ecam_right; 636 | cv::cv2eigen(camera_matrix_left, ecam_left); 637 | cv::cv2eigen(camera_matrix_right, ecam_right); 638 | Matrix3f rotation = A_right.block<3, 3>(0, 0); 639 | Eigen::MatrixXf translation = A_right.block<3, 1>(0,3); 640 | Matrix A = ecam_left * rotation.transpose() * translation; 641 | Matrix3f C; 642 | C << 0.0, -A[3], A[2], A[3], 0.0, -A[1], -A[2], A[1], 0.0; 643 | fundamental = 644 | ecam_right.inverse().transpose() * rotation * ecam_left.transpose() * C; 645 | if (false) { 646 | std::cout << "\n\nprojection_left:\n"; 647 | std::cout << projection_left << "\n"; 648 | std::cout << "\n\nprojection_right:\n"; 649 | std::cout << projection_right<< "\n"; 650 | exit(0); 651 | } 652 | } 653 | 654 | } // namespace slam 655 | --------------------------------------------------------------------------------