├── Linear-Kalman-Filter ├── CMakeLists.txt ├── README.md ├── include │ ├── Load_Params.h │ ├── Prediction.h │ ├── Processing.h │ └── Update.h ├── launch │ ├── KF.launch │ └── Params.yaml ├── package.xml └── src │ └── Main.cpp ├── README.md ├── Surge Report.pdf ├── camera_localization ├── imu_tools │ ├── README.md │ ├── imu_complementary_filter │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── imu_complementary_filter │ │ │ │ ├── complementary_filter.h │ │ │ │ └── complementary_filter_ros.h │ │ ├── launch │ │ │ └── complementary_filter.launch │ │ ├── package.xml │ │ └── src │ │ │ ├── complementary_filter.cpp │ │ │ ├── complementary_filter_node.cpp │ │ │ └── complementary_filter_ros.cpp │ ├── imu_filter_madgwick │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── COPYING │ │ ├── Makefile │ │ ├── cfg │ │ │ └── ImuFilterMadgwick.cfg │ │ ├── imu_filter_nodelet.xml │ │ ├── include │ │ │ └── imu_filter_madgwick │ │ │ │ ├── imu_filter.h │ │ │ │ ├── imu_filter.launch │ │ │ │ ├── imu_filter_nodelet.h │ │ │ │ ├── imu_filter_ros.h │ │ │ │ ├── stateless_orientation.h │ │ │ │ └── world_frame.h │ │ ├── package.xml │ │ ├── sample │ │ │ ├── ardrone_imu.bag │ │ │ ├── phidgets_imu_upside_down.bag │ │ │ └── sparkfun_razor.bag │ │ ├── src │ │ │ ├── imu_filter.cpp │ │ │ ├── imu_filter_node.cpp │ │ │ ├── imu_filter_nodelet.cpp │ │ │ ├── imu_filter_ros.cpp │ │ │ └── stateless_orientation.cpp │ │ └── test │ │ │ ├── madgwick_test.cpp │ │ │ ├── stateless_orientation_test.cpp │ │ │ └── test_helpers.h │ ├── imu_tools │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ └── rviz_imu_plugin │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ ├── plugin_description.xml │ │ ├── rosdoc.yaml │ │ ├── rviz_imu_plugin.png │ │ └── src │ │ ├── imu_acc_visual.cpp │ │ ├── imu_acc_visual.h │ │ ├── imu_axes_visual.cpp │ │ ├── imu_axes_visual.h │ │ ├── imu_display.cpp │ │ ├── imu_display.h │ │ ├── imu_orientation_visual.cpp │ │ └── imu_orientation_visual.h └── localization │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ ├── odom_node.cpp │ ├── realsense_comb_imu.cpp │ └── tf_publisher.cpp ├── octomap_mapping ├── README.md ├── octomap_mapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml └── octomap_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── cfg │ └── OctomapServer.cfg │ ├── include │ └── octomap_server │ │ ├── OctomapServer.h │ │ ├── OctomapServerMultilayer.h │ │ └── TrackingOctomapServer.h │ ├── launch │ ├── octomap_mapping.launch │ ├── octomap_mapping_nodelet.launch │ ├── octomap_tracking_client.launch │ └── octomap_tracking_server.launch │ ├── mainpage.dox │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── params │ └── default.yaml │ ├── scripts │ └── octomap_eraser_cli.py │ └── src │ ├── OctomapServer.cpp │ ├── OctomapServerMultilayer.cpp │ ├── TrackingOctomapServer.cpp │ ├── octomap_saver.cpp │ ├── octomap_server_multilayer.cpp │ ├── octomap_server_node.cpp │ ├── octomap_server_nodelet.cpp │ ├── octomap_server_static.cpp │ └── octomap_tracking_server_node.cpp ├── octomap_saver └── map_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ └── map_server │ │ └── image_loader.h │ ├── package.xml │ ├── scripts │ └── crop_map │ ├── src │ ├── image_loader.cpp │ ├── main.cpp │ ├── map_saver.cpp │ └── map_server.dox │ └── test │ ├── consumer.py │ ├── rtest.cpp │ ├── rtest.xml │ ├── test_constants.cpp │ ├── test_constants.h │ ├── testmap.bmp │ ├── testmap.png │ ├── testmap.yaml │ └── utest.cpp ├── orb_slam_2_ros ├── .gitignore ├── CMakeLists.txt ├── Dependencies.md ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── orb_slam2 │ ├── Thirdparty │ │ ├── DBoW2 │ │ │ ├── CMakeLists.txt │ │ │ ├── DBoW2 │ │ │ │ ├── BowVector.cpp │ │ │ │ ├── BowVector.h │ │ │ │ ├── FClass.h │ │ │ │ ├── FORB.cpp │ │ │ │ ├── FORB.h │ │ │ │ ├── FeatureVector.cpp │ │ │ │ ├── FeatureVector.h │ │ │ │ ├── ScoringObject.cpp │ │ │ │ ├── ScoringObject.h │ │ │ │ └── TemplatedVocabulary.h │ │ │ ├── DUtils │ │ │ │ ├── Random.cpp │ │ │ │ ├── Random.h │ │ │ │ ├── Timestamp.cpp │ │ │ │ └── Timestamp.h │ │ │ ├── LICENSE.txt │ │ │ └── README.txt │ │ └── g2o │ │ │ ├── CMakeLists.txt │ │ │ ├── README.txt │ │ │ ├── cmake_modules │ │ │ ├── FindBLAS.cmake │ │ │ ├── FindEigen3.cmake │ │ │ └── FindLAPACK.cmake │ │ │ ├── config.h │ │ │ ├── config.h.in │ │ │ ├── g2o │ │ │ ├── core │ │ │ │ ├── base_binary_edge.h │ │ │ │ ├── base_binary_edge.hpp │ │ │ │ ├── base_edge.h │ │ │ │ ├── base_multi_edge.h │ │ │ │ ├── base_multi_edge.hpp │ │ │ │ ├── base_unary_edge.h │ │ │ │ ├── base_unary_edge.hpp │ │ │ │ ├── base_vertex.h │ │ │ │ ├── base_vertex.hpp │ │ │ │ ├── batch_stats.cpp │ │ │ │ ├── batch_stats.h │ │ │ │ ├── block_solver.h │ │ │ │ ├── block_solver.hpp │ │ │ │ ├── cache.cpp │ │ │ │ ├── cache.h │ │ │ │ ├── creators.h │ │ │ │ ├── eigen_types.h │ │ │ │ ├── estimate_propagator.cpp │ │ │ │ ├── estimate_propagator.h │ │ │ │ ├── factory.cpp │ │ │ │ ├── factory.h │ │ │ │ ├── hyper_dijkstra.cpp │ │ │ │ ├── hyper_dijkstra.h │ │ │ │ ├── hyper_graph.cpp │ │ │ │ ├── hyper_graph.h │ │ │ │ ├── hyper_graph_action.cpp │ │ │ │ ├── hyper_graph_action.h │ │ │ │ ├── jacobian_workspace.cpp │ │ │ │ ├── jacobian_workspace.h │ │ │ │ ├── linear_solver.h │ │ │ │ ├── marginal_covariance_cholesky.cpp │ │ │ │ ├── marginal_covariance_cholesky.h │ │ │ │ ├── matrix_operations.h │ │ │ │ ├── matrix_structure.cpp │ │ │ │ ├── matrix_structure.h │ │ │ │ ├── openmp_mutex.h │ │ │ │ ├── optimizable_graph.cpp │ │ │ │ ├── optimizable_graph.h │ │ │ │ ├── optimization_algorithm.cpp │ │ │ │ ├── optimization_algorithm.h │ │ │ │ ├── optimization_algorithm_dogleg.cpp │ │ │ │ ├── optimization_algorithm_dogleg.h │ │ │ │ ├── optimization_algorithm_factory.cpp │ │ │ │ ├── optimization_algorithm_factory.h │ │ │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ │ │ ├── optimization_algorithm_gauss_newton.h │ │ │ │ ├── optimization_algorithm_levenberg.cpp │ │ │ │ ├── optimization_algorithm_levenberg.h │ │ │ │ ├── optimization_algorithm_property.h │ │ │ │ ├── optimization_algorithm_with_hessian.cpp │ │ │ │ ├── optimization_algorithm_with_hessian.h │ │ │ │ ├── parameter.cpp │ │ │ │ ├── parameter.h │ │ │ │ ├── parameter_container.cpp │ │ │ │ ├── parameter_container.h │ │ │ │ ├── robust_kernel.cpp │ │ │ │ ├── robust_kernel.h │ │ │ │ ├── robust_kernel_factory.cpp │ │ │ │ ├── robust_kernel_factory.h │ │ │ │ ├── robust_kernel_impl.cpp │ │ │ │ ├── robust_kernel_impl.h │ │ │ │ ├── solver.cpp │ │ │ │ ├── solver.h │ │ │ │ ├── sparse_block_matrix.h │ │ │ │ ├── sparse_block_matrix.hpp │ │ │ │ ├── sparse_block_matrix_ccs.h │ │ │ │ ├── sparse_block_matrix_diagonal.h │ │ │ │ ├── sparse_block_matrix_test.cpp │ │ │ │ ├── sparse_optimizer.cpp │ │ │ │ └── sparse_optimizer.h │ │ │ ├── solvers │ │ │ │ ├── linear_solver_dense.h │ │ │ │ └── linear_solver_eigen.h │ │ │ ├── stuff │ │ │ │ ├── color_macros.h │ │ │ │ ├── macros.h │ │ │ │ ├── misc.h │ │ │ │ ├── os_specific.c │ │ │ │ ├── os_specific.h │ │ │ │ ├── property.cpp │ │ │ │ ├── property.h │ │ │ │ ├── string_tools.cpp │ │ │ │ ├── string_tools.h │ │ │ │ ├── timeutil.cpp │ │ │ │ └── timeutil.h │ │ │ └── types │ │ │ │ ├── se3_ops.h │ │ │ │ ├── se3_ops.hpp │ │ │ │ ├── se3quat.h │ │ │ │ ├── sim3.h │ │ │ │ ├── types_sba.cpp │ │ │ │ ├── types_sba.h │ │ │ │ ├── types_seven_dof_expmap.cpp │ │ │ │ ├── types_seven_dof_expmap.h │ │ │ │ ├── types_six_dof_expmap.cpp │ │ │ │ └── types_six_dof_expmap.h │ │ │ └── license-bsd.txt │ ├── Vocabulary │ │ ├── ORBvoc.txt.bin │ │ └── ORBvoc.txt.tar.gz │ ├── cmake_modules │ │ └── FindEigen3.cmake │ ├── config │ │ ├── RealSenseD435Mono.yaml │ │ ├── RealSenseD435RGBD.yaml │ │ ├── RealSenseR200Mono.yaml │ │ ├── RealSenseR200RGBD.yaml │ │ ├── RealSenseR200Stereo.yaml │ │ ├── TUM1.yaml │ │ ├── mynteye_s_mono.yaml │ │ ├── mynteye_s_stereo.yaml │ │ └── vimba_stereo.yaml │ ├── include │ │ ├── Converter.h │ │ ├── Frame.h │ │ ├── FrameDrawer.h │ │ ├── Initializer.h │ │ ├── KeyFrame.h │ │ ├── KeyFrameDatabase.h │ │ ├── LocalMapping.h │ │ ├── LoopClosing.h │ │ ├── Map.h │ │ ├── MapPoint.h │ │ ├── ORBVocabulary.h │ │ ├── ORBextractor.h │ │ ├── ORBmatcher.h │ │ ├── Optimizer.h │ │ ├── PnPsolver.h │ │ ├── Sim3Solver.h │ │ ├── System.h │ │ └── Tracking.h │ └── src │ │ ├── Converter.cc │ │ ├── Frame.cc │ │ ├── FrameDrawer.cc │ │ ├── Initializer.cc │ │ ├── KeyFrame.cc │ │ ├── KeyFrameDatabase.cc │ │ ├── LocalMapping.cc │ │ ├── LoopClosing.cc │ │ ├── Map.cc │ │ ├── MapPoint.cc │ │ ├── ORBextractor.cc │ │ ├── ORBmatcher.cc │ │ ├── Optimizer.cc │ │ ├── PnPsolver.cc │ │ ├── Sim3Solver.cc │ │ ├── System.cc │ │ └── Tracking.cc ├── package.xml └── ros │ ├── config │ └── dynamic_reconfigure.cfg │ ├── include │ ├── MonoNode.h │ ├── Node.h │ ├── RGBDNode.h │ └── StereoNode.h │ ├── launch │ ├── orb_slam2_d435_mono.launch │ ├── orb_slam2_d435_rgbd.launch │ ├── orb_slam2_mynteye_s_mono.launch │ ├── orb_slam2_mynteye_s_stereo.launch │ ├── orb_slam2_r200_mono.launch │ ├── orb_slam2_r200_rgbd.launch │ └── orb_slam2_r200_stereo.launch │ └── src │ ├── MonoNode.cc │ ├── Node.cc │ ├── RGBDNode.cc │ └── StereoNode.cc ├── ptcldfiltering ├── CMakeLists.txt ├── package.xml └── src │ └── ptcldfilter_node.cpp ├── quad.urdf ├── slam3d ├── CMakeLists.txt ├── include │ └── slam3d │ │ └── slam3d.launch └── package.xml └── timed_roslaunch ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── launch ├── example.launch.sample ├── timed_roslaunch.launch └── timed_roslaunch_test.launch ├── package.xml ├── scripts └── timed_roslaunch.sh ├── test ├── runtest.launch ├── test.py └── timed_roslaunch.test.xml └── timed_roslaunch.sh /Linear-Kalman-Filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(filter) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | sensor_msgs 8 | std_msgs 9 | cmake_modules 10 | 11 | 12 | ) 13 | 14 | find_package(Eigen REQUIRED) 15 | #find_package(OpenCV REQUIRED) 16 | #find_package(Boost REQUIRED COMPONENTS system) 17 | #find_package(PCL REQUIRED COMPONENTS common io) 18 | 19 | catkin_package( 20 | DEPENDS Eigen 21 | INCLUDE_DIRS include 22 | CATKIN_DEPENDS roscpp 23 | ) 24 | include_directories(include 25 | ${catkin_INCLUDE_DIRS} 26 | ${Eigen_INCLUDE_DIRS}) 27 | 28 | ## Declare a cpp executable 29 | add_executable(KalmanFilter src/Main.cpp) 30 | target_link_libraries(KalmanFilter 31 | ${catkin_LIBRARIES}) 32 | -------------------------------------------------------------------------------- /Linear-Kalman-Filter/include/Prediction.h: -------------------------------------------------------------------------------- 1 | /* 2 | Call Back function for obtaining the control vector 3 | The argument can be changed to that of incoming message 4 | */ 5 | 6 | void Prediction_step(const sensor_msgs::Imu::ConstPtr& control){ 7 | 8 | Uk=UkProcess(*control); 9 | 10 | //Predicting belief of x 11 | Xhatk=Fk*Xk+Bk*Uk; 12 | 13 | //Predicting covariance of x 14 | CovarhatX=(Fk*CovarX)*Fk.transpose()+Qk; 15 | 16 | //Calculating kalman gain 17 | MatrixXf temp2,temp3; 18 | temp2=(Hk*CovarhatX)*Hk.transpose()+Rk; 19 | temp3=temp2.inverse(); 20 | KGain=(CovarhatX*Hk.transpose())*temp3; 21 | 22 | std::cout << "Yo" << std::endl; 23 | 24 | // std::cout << "Predict" << std::endl; 25 | } -------------------------------------------------------------------------------- /Linear-Kalman-Filter/include/Processing.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | //Necessary includes for the required messages. 10 | #include 11 | #include 12 | 13 | using namespace Eigen; 14 | 15 | //Output to be published. Can be of different type for different needs 16 | geometry_msgs::PoseStamped output; 17 | 18 | 19 | /* 20 | The Following functions are for processing the various vectors. Please change them according to needs 21 | */ 22 | 23 | //Function for the preprocessing of the control vector. Takes the control message(control) and returns Uk(result) 24 | MatrixXf UkProcess(sensor_msgs::Imu control){ 25 | 26 | /* 27 | Miscellaneous vectors/matrices for converting incoming data into a useable form. 28 | Can be omitted/changed according to needs 29 | */ 30 | MatrixXf g(3,1); 31 | MatrixXf imutemp(3,1); 32 | 33 | MatrixXf result(3,1); 34 | 35 | float Ax,Ay,Az; 36 | float Angx,Angy,Angz; 37 | Quaternionf q,qrev; 38 | q = Quaternionf(control.orientation.w, control.orientation.x, control.orientation.y, control.orientation.z); 39 | MatrixXf rotmat = q.toRotationMatrix(); 40 | MatrixXf rotmat2=rotmat.inverse(); 41 | 42 | Ax=control.linear_acceleration.x; 43 | Ay=control.linear_acceleration.y; 44 | Az=control.linear_acceleration.z; 45 | imutemp << Ax,Ay,Az; 46 | g << 0,0,-9.8; 47 | result =imutemp+rotmat2*g; 48 | return result; 49 | } 50 | 51 | //Function for the preprocessing of the measurement vector. Takes the measurement message(measurement) and returns Zk(result) 52 | MatrixXf ZkProcess(const geometry_msgs::PoseStamped measurement){ 53 | 54 | MatrixXf result; 55 | result << measurement.pose.position.x,measurement.pose.position.y,measurement.pose.position.z; 56 | return result; 57 | } 58 | 59 | //Function for the preprocessing of the output message. Takes the measurement message(measure) and Belief(Xk) and returns output message(result) 60 | geometry_msgs::PoseStamped OutputProcess (MatrixXf Belief,geometry_msgs::PoseStamped measure){ 61 | 62 | geometry_msgs::PoseStamped result; 63 | 64 | result.header.stamp=measure.header.stamp; 65 | result.header.seq=measure.header.seq; 66 | result.header.frame_id=measure.header.frame_id; 67 | result.pose.orientation.x=measure.pose.orientation.x; 68 | result.pose.orientation.y=measure.pose.orientation.y; 69 | result.pose.orientation.z=measure.pose.orientation.z; 70 | result.pose.orientation.w=measure.pose.orientation.w; 71 | 72 | result.pose.position.x=Belief(0,0); 73 | result.pose.position.y=Belief(1,0); 74 | result.pose.position.z=Belief(2,0); 75 | 76 | return result; 77 | } 78 | 79 | -------------------------------------------------------------------------------- /Linear-Kalman-Filter/include/Update.h: -------------------------------------------------------------------------------- 1 | /* 2 | Call Back function for obtaining the measurement vector. 3 | The argument can be changed to that of incoming message 4 | */ 5 | 6 | void Update_step(const geometry_msgs::PoseStamped::ConstPtr& measurement){ 7 | 8 | //Preproccessing for making the output message ready for publishing. Can be changed/omitted 9 | std::cout << "HI2" << std::endl; 10 | Zk << measurement->pose.position.x,measurement->pose.position.y,measurement->pose.position.z; 11 | 12 | //For the first value, set initial belief. Can be set to whatever you fancy 13 | static int ifFirst=1; 14 | if (ifFirst){ 15 | ifFirst=0; 16 | Xk << measurement->pose.position.x, 17 | measurement->pose.position.y, 18 | measurement->pose.position.z, 19 | 0, 20 | 0, 21 | 0; 22 | return; 23 | } 24 | //Calculating new belief of x 25 | Xk=Xhatk+KGain*(Zk-(Hk*Xhatk)); 26 | std::cout << CovarhatX << std::endl; 27 | // std::cout << CovarhatX << std::endl; 28 | //Calculating new covariance of x 29 | CovarX=CovarhatX-((KGain*Hk)*CovarhatX); 30 | output=OutputProcess(Xk,*measurement); 31 | } -------------------------------------------------------------------------------- /Linear-Kalman-Filter/launch/KF.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /Linear-Kalman-Filter/src/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "Processing.h" 2 | #include "Load_Params.h" 3 | #include "Update.h" 4 | #include "Prediction.h" 5 | 6 | int main(int argc, char** argv){ 7 | 8 | ros::init(argc,argv,"KalmanFilter"); 9 | ros::NodeHandle nh; 10 | 11 | //Loading the parameters 12 | Load_Params(nh); 13 | 14 | //Subscriber and Publisher for the data. Can be remapped in the launch file to the topic required 15 | ros::Subscriber Image=nh.subscribe("Measurement_data",100,Update_step); 16 | ros::Subscriber Imu=nh.subscribe("Prediction_data",100,Prediction_step); 17 | ros::Publisher fused = nh.advertise("Filtered_data", 10); 18 | 19 | ros::Rate loop_rate(100); 20 | while(ros::ok()){ 21 | fused.publish(output); 22 | // std::cout << CovarhatX << std::endl; 23 | ros::spinOnce(); 24 | loop_rate.sleep(); 25 | } 26 | return 0; 27 | } 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # slam-octomap 2 | 3 | Tested with Intel Realsense D435i stereo camera. Capable of localizing in GPS denied environment while building a 3-D map of the surrounding. 4 | 5 | Installation Instructions - Ubuntu 16.04 with ROS Kinetic 6 | --------------------------------------------------------- 7 | 8 | 1. First install ROS Kinetic in your system following this guide, prefer Ubuntu 16.04 9 | 10 | [Ubuntu Installation](https://aerial-robotics-iitk.gitbook.io/wiki/tutorials/workspace-setup/installing-ubuntu) 11 | 12 | [ROS Installation](https://aerial-robotics-iitk.gitbook.io/wiki/tutorials/workspace-setup/ros-setup) 13 | 14 | 2. Install additional libraries 15 | ``` 16 | sudo apt-get install ros-kinetic-octomap-ros 17 | ``` 18 | 19 | * If you are using Intel Realsense install SDK and driver first from [here](https://github.com/AerialRobotics-IITK/realsense.git) 20 | 21 | 3. Setup your workspace 22 | 23 | ``` 24 | mkdir -p ~/catkin_ws/src 25 | cd ~/catkin_ws/src 26 | git clone https://github.com/da-piyushpatel/slam-octomap.git 27 | cd ~/catkin_ws 28 | catkin build 29 | echo "~/catkin_ws/devel/setup.bash" 30 | ``` 31 | 32 | Sources 33 | ------- 34 | The realsense camera is provided with accelerometer and gyroscope, using this algorithm onc can find out cmaera's orientation in 3D space 35 | 36 | * [imu_filter_madgwick](https://x-io.co.uk/open-source-imu-and-ahrs-algorithms) 37 | 38 | * https://github.com/IntelRealSense/realsenense-ros 39 | * http://wiki.ros.org/imu_filter_madgwick 40 | 41 | `roslaunch realsense2_camera rs_camera.launch unite_imu_method:=copy filters:=pointcloud` 42 | 43 | ` rosrun imu_filter_madgwick imu_filter_node _use_magnetic_field_msg:=false _publish_debug_topics:=true` 44 | 45 | * To know more about the codes and this project see the attached SURGE_report.pdf 46 | -------------------------------------------------------------------------------- /Surge Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/Surge Report.pdf -------------------------------------------------------------------------------- /camera_localization/imu_tools/README.md: -------------------------------------------------------------------------------- 1 | IMU tools for ROS 2 | =================================== 3 | 4 | Overview 5 | ----------------------------------- 6 | 7 | IMU-related filters and visualizers. The stack contains: 8 | 9 | * `imu_filter_madgwick`: a filter which fuses angular velocities, 10 | accelerations, and (optionally) magnetic readings from a generic IMU 11 | device into an orientation. Based on the work of [1]. 12 | 13 | * `imu_complementary_filter`: a filter which fuses angular velocities, 14 | accelerations, and (optionally) magnetic readings from a generic IMU 15 | device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2]. 16 | 17 | * `rviz_imu_plugin` a plugin for rviz which displays `sensor_msgs::Imu` 18 | messages 19 | 20 | Installing 21 | ----------------------------------- 22 | 23 | ### From source ### 24 | 25 | [Create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) 26 | (e.g., `~/ros-hydro-ws/`) and source the `devel/setup.bash` file. 27 | 28 | Make sure you have git installed: 29 | 30 | sudo apt-get install git-core 31 | 32 | Download the stack from our repository into your catkin workspace (e.g., 33 | `ros-hydro-ws/src`; use the proper branch for your distro, e.g., `groovy`, 34 | `hydro`...): 35 | 36 | git clone -b https://github.com/ccny-ros-pkg/imu_tools.git 37 | 38 | Install any dependencies using [rosdep](http://www.ros.org/wiki/rosdep). 39 | 40 | rosdep install imu_tools 41 | 42 | Compile the stack: 43 | 44 | cd ~/ros-hydro-ws 45 | catkin_make 46 | 47 | More info 48 | ----------------------------------- 49 | 50 | http://wiki.ros.org/imu_tools 51 | 52 | License 53 | ----------------------------------- 54 | 55 | * `imu_filter_madgwick`: currently licensed as GPL, following the original implementation 56 | 57 | * `imu_complementary_filter`: BSD 58 | 59 | * `rviz_imu_plugin`: BSD 60 | 61 | References 62 | ----------------------------------- 63 | [1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 64 | 65 | [2] http://www.mdpi.com/1424-8220/15/8/19302 66 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_complementary_filter/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_complementary_filter 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.15 (2017-05-22) 6 | ------------------- 7 | 8 | 1.0.14 (2017-03-10) 9 | ------------------- 10 | * complementary_filter: move const initializations out of header 11 | Initialization of static consts other than int (here: float) inside the 12 | class declaration is not permitted in C++. It works in gcc (due to a 13 | non-standard extension), but throws an error in C++11. 14 | * Contributors: Martin Guenther 15 | 16 | 1.0.13 (2016-09-07) 17 | ------------------- 18 | 19 | 1.0.12 (2016-09-07) 20 | ------------------- 21 | 22 | 1.0.10 (2016-04-22) 23 | ------------------- 24 | * Remove Eigen dependency 25 | Eigen is not actually used anywhere. Thanks @asimay! 26 | * Removed main function from shared library 27 | * Contributors: Martin Guenther, Matthias Nieuwenhuisen 28 | 29 | 1.0.9 (2015-10-16) 30 | ------------------ 31 | * complementary: Add Eigen dependency 32 | Fixes `#54 `_. 33 | * Contributors: Martin Günther 34 | 35 | 1.0.8 (2015-10-07) 36 | ------------------ 37 | 38 | 1.0.7 (2015-10-07) 39 | ------------------ 40 | * Allow remapping imu namespace 41 | * Publish RPY as Vector3Stamped 42 | * Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics 43 | * Use MagneticField instead of Vector3 44 | * Contributors: Martin Günther 45 | 46 | 1.0.6 (2015-10-06) 47 | ------------------ 48 | * Add new package: imu_complementary_filter 49 | * Contributors: Roberto G. Valentini, Martin Günther, Michael Görner 50 | 51 | 1.0.5 (2015-06-24) 52 | ------------------ 53 | 54 | 1.0.4 (2015-05-06) 55 | ------------------ 56 | 57 | 1.0.3 (2015-01-29) 58 | ------------------ 59 | 60 | 1.0.2 (2015-01-27) 61 | ------------------ 62 | 63 | 1.0.1 (2014-12-10) 64 | ------------------ 65 | 66 | 1.0.0 (2014-11-28) 67 | ------------------ 68 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_complementary_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_complementary_filter) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cmake_modules 6 | message_filters 7 | roscpp 8 | sensor_msgs 9 | std_msgs 10 | tf 11 | ) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | LIBRARIES complementary_filter 16 | CATKIN_DEPENDS message_filters roscpp sensor_msgs std_msgs tf 17 | ) 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | ## Declare a cpp library 25 | add_library(complementary_filter 26 | src/complementary_filter.cpp 27 | src/complementary_filter_ros.cpp 28 | include/imu_complementary_filter/complementary_filter.h 29 | include/imu_complementary_filter/complementary_filter_ros.h 30 | ) 31 | 32 | 33 | # create complementary_filter_node executable 34 | add_executable(complementary_filter_node 35 | src/complementary_filter_node.cpp) 36 | target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES}) 37 | 38 | install(TARGETS complementary_filter complementary_filter_node 39 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 41 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 42 | ) 43 | 44 | ## Mark cpp header files for installation 45 | install(DIRECTORY include/${PROJECT_NAME}/ 46 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 47 | FILES_MATCHING PATTERN "*.h" 48 | PATTERN ".svn" EXCLUDE 49 | ) 50 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_complementary_filter/launch/complementary_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | #### Nodelet manager ###################################################### 4 | 5 | 7 | 8 | #### IMU Driver ########################################################### 9 | 10 | 13 | 14 | # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms) 15 | 16 | 17 | 18 | 19 | #### Complementary filter 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_complementary_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_complementary_filter 4 | 1.0.15 5 | Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 . 6 | 7 | Roberto G. Valenti 8 | BSD 9 | 10 | http://www.mdpi.com/1424-8220/15/8/19302 11 | Roberto G. Valenti --> 12 | 13 | catkin 14 | cmake_modules 15 | message_filters 16 | roscpp 17 | sensor_msgs 18 | std_msgs 19 | tf 20 | message_filters 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | 26 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_complementary_filter/src/complementary_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 1. Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | 2. Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 3. Neither the name of the City College of New York nor the 17 | names of its contributors may be used to endorse or promote products 18 | derived from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "imu_complementary_filter/complementary_filter_ros.h" 33 | 34 | int main (int argc, char **argv) 35 | { 36 | ros::init (argc, argv, "ComplementaryFilterROS"); 37 | ros::NodeHandle nh; 38 | ros::NodeHandle nh_private("~"); 39 | imu_tools::ComplementaryFilterROS filter(nh, nh_private); 40 | ros::spin(); 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_filter_madgwick) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs tf2 tf2_geometry_msgs tf2_ros nodelet pluginlib message_filters dynamic_reconfigure) 5 | 6 | find_package(Boost REQUIRED COMPONENTS system thread signals) 7 | 8 | # Generate dynamic parameters 9 | generate_dynamic_reconfigure_options(cfg/ImuFilterMadgwick.cfg) 10 | 11 | 12 | catkin_package( 13 | DEPENDS Boost 14 | CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs tf2_ros tf2_geometry_msgs nodelet pluginlib message_filters dynamic_reconfigure 15 | INCLUDE_DIRS 16 | LIBRARIES imu_filter imu_filter_nodelet 17 | ) 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ${Boost_INCLUDE_DIRS} 23 | ) 24 | 25 | 26 | # create imu_filter library 27 | add_library (imu_filter src/imu_filter.cpp src/imu_filter_ros.cpp src/stateless_orientation.cpp) 28 | add_dependencies(imu_filter ${PROJECT_NAME}_gencfg) 29 | target_link_libraries(imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 30 | 31 | # create imu_filter_nodelet library 32 | add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp) 33 | add_dependencies(imu_filter_nodelet ${PROJECT_NAME}_gencfg) 34 | target_link_libraries(imu_filter_nodelet imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 35 | 36 | # create imu_filter_node executable 37 | add_executable(imu_filter_node src/imu_filter_node.cpp) 38 | add_dependencies(imu_filter_node ${PROJECT_NAME}_gencfg) 39 | target_link_libraries(imu_filter_node imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 40 | 41 | 42 | install(TARGETS imu_filter imu_filter_nodelet imu_filter_node 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | FILES_MATCHING PATTERN "*.h" 51 | ) 52 | 53 | install(FILES imu_filter_nodelet.xml 54 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 55 | ) 56 | 57 | 58 | 59 | if(CATKIN_ENABLE_TESTING) 60 | catkin_add_gtest(${PROJECT_NAME}-madgwick_test 61 | test/stateless_orientation_test.cpp 62 | test/madgwick_test.cpp 63 | ) 64 | target_link_libraries(${PROJECT_NAME}-madgwick_test 65 | imu_filter 66 | ${catkin_LIBRARIES} 67 | ) 68 | endif() 69 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # IMU Filter dynamic reconfigure 3 | 4 | PACKAGE='imu_filter_madgwick' 5 | 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("gain", double_t, 0, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal.", 0.1, 0.0, 1.0) 11 | gen.add("zeta", double_t, 0, "Gyro drift gain (approx. rad/s).", 0, -1.0, 1.0) 12 | gen.add("mag_bias_x", double_t, 0, "Magnetometer bias (hard iron correction), x component.", 0, -10.0, 10.0) 13 | gen.add("mag_bias_y", double_t, 0, "Magnetometer bias (hard iron correction), y component.", 0, -10.0, 10.0) 14 | gen.add("mag_bias_z", double_t, 0, "Magnetometer bias (hard iron correction), z component.", 0, -10.0, 10.0) 15 | gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 1.0) 16 | 17 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ImuFilterMadgwick")) 18 | 19 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/imu_filter_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | Imu Filter nodelet publisher. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 26 | #define IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 27 | 28 | #include 29 | 30 | #include "imu_filter_madgwick/imu_filter_ros.h" 31 | 32 | class ImuFilterNodelet : public nodelet::Nodelet 33 | { 34 | public: 35 | virtual void onInit(); 36 | 37 | private: 38 | boost::shared_ptr filter_; 39 | }; 40 | 41 | #endif // IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 42 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 26 | #define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | class StatelessOrientation 33 | { 34 | public: 35 | static bool computeOrientation( 36 | WorldFrame::WorldFrame frame, 37 | geometry_msgs::Vector3 acceleration, 38 | geometry_msgs::Vector3 magneticField, 39 | geometry_msgs::Quaternion& orientation); 40 | 41 | static bool computeOrientation( 42 | WorldFrame::WorldFrame frame, 43 | geometry_msgs::Vector3 acceleration, 44 | geometry_msgs::Quaternion& orientation); 45 | 46 | }; 47 | 48 | #endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 49 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 2 | #define IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 3 | 4 | namespace WorldFrame { 5 | enum WorldFrame { ENU, NED, NWU }; 6 | } 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | imu_filter_madgwick 3 | 1.0.15 4 | 5 | Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms. 6 | 7 | GPL 8 | http://ros.org/wiki/imu_filter_madgwick 9 | Ivan Dryanovski 10 | Martin Günther 11 | Ivan Dryanovski 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | geometry_msgs 18 | tf2 19 | tf2_geometry_msgs 20 | tf2_ros 21 | nodelet 22 | pluginlib 23 | message_filters 24 | dynamic_reconfigure 25 | 26 | roscpp 27 | sensor_msgs 28 | geometry_msgs 29 | tf2 30 | tf2_geometry_msgs 31 | tf2_ros 32 | nodelet 33 | pluginlib 34 | message_filters 35 | dynamic_reconfigure 36 | 37 | rosunit 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/sample/ardrone_imu.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/camera_localization/imu_tools/imu_filter_madgwick/sample/ardrone_imu.bag -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/sample/phidgets_imu_upside_down.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/camera_localization/imu_tools/imu_filter_madgwick/sample/phidgets_imu_upside_down.bag -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/sample/sparkfun_razor.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/camera_localization/imu_tools/imu_filter_madgwick/sample/sparkfun_razor.bag -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/src/imu_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/imu_filter_ros.h" 26 | 27 | int main (int argc, char **argv) 28 | { 29 | ros::init (argc, argv, "ImuFilter"); 30 | ros::NodeHandle nh; 31 | ros::NodeHandle nh_private("~"); 32 | ImuFilterRos imu_filter(nh, nh_private); 33 | ros::spin(); 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_filter_madgwick/src/imu_filter_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/imu_filter_nodelet.h" 26 | #include 27 | 28 | void ImuFilterNodelet::onInit() 29 | { 30 | NODELET_INFO("Initializing IMU Filter Nodelet"); 31 | 32 | // TODO: Do we want the single threaded or multithreaded NH? 33 | ros::NodeHandle nh = getMTNodeHandle(); 34 | ros::NodeHandle nh_private = getMTPrivateNodeHandle(); 35 | 36 | filter_.reset(new ImuFilterRos(nh, nh_private)); 37 | } 38 | 39 | PLUGINLIB_DECLARE_CLASS(imu_filter_madgwick, ImuFilterNodelet, ImuFilterNodelet, nodelet::Nodelet); 40 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.15 (2017-05-22) 6 | ------------------- 7 | 8 | 1.0.14 (2017-03-10) 9 | ------------------- 10 | 11 | 1.0.13 (2016-09-07) 12 | ------------------- 13 | 14 | 1.0.12 (2016-09-07) 15 | ------------------- 16 | 17 | 1.0.10 (2016-04-22) 18 | ------------------- 19 | 20 | 1.0.9 (2015-10-16) 21 | ------------------ 22 | 23 | 1.0.8 (2015-10-07) 24 | ------------------ 25 | * Add imu_complementary_filter to meta package 26 | * Contributors: Martin Günther 27 | 28 | 1.0.7 (2015-10-07) 29 | ------------------ 30 | 31 | 1.0.6 (2015-10-06) 32 | ------------------ 33 | 34 | 1.0.5 (2015-06-24) 35 | ------------------ 36 | 37 | 1.0.4 (2015-05-06) 38 | ------------------ 39 | 40 | 1.0.3 (2015-01-29) 41 | ------------------ 42 | 43 | 1.0.2 (2015-01-27) 44 | ------------------ 45 | 46 | 1.0.1 (2014-12-10) 47 | ------------------ 48 | * add me as maintainer to package.xml 49 | * Contributors: Martin Günther 50 | 51 | 1.0.0 (2014-09-03) 52 | ------------------ 53 | * First public release 54 | * catkinization of imu_tools metapackage 55 | * Contributors: Francisco Vina 56 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/imu_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | imu_tools 3 | 1.0.15 4 | 5 | Various tools for IMU devices 6 | 7 | Martin Günther 8 | Ivan Dryanovski 9 | BSD, GPL 10 | 11 | http://ros.org/wiki/imu_tools 12 | 13 | catkin 14 | 15 | imu_complementary_filter 16 | imu_filter_madgwick 17 | rviz_imu_plugin 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_imu_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.15 (2017-05-22) 6 | ------------------- 7 | * Add option to display orientation in world frame (`#69 `_) 8 | Per REP 145 IMU orientation is in the world frame. Rotating the 9 | orientation data to transform into the sensor frame results in strange 10 | behavior, such as double-rotation of orientation on a robot. Provide an 11 | option to display orientation in the world frame, and enable it by 12 | default. Continue to translate the position of the data to the sensor 13 | frame. 14 | * Contributors: C. Andy Martin 15 | 16 | 1.0.14 (2017-03-10) 17 | ------------------- 18 | 19 | 1.0.13 (2016-09-07) 20 | ------------------- 21 | 22 | 1.0.12 (2016-09-07) 23 | ------------------- 24 | 25 | 1.0.10 (2016-04-22) 26 | ------------------- 27 | * Support qt4/qt5 using rviz's exported qt version 28 | Closes `#58 `_ . 29 | This fixes the build on Kinetic, where only Qt5 is available, and 30 | is backwards compatible with Qt4 for Indigo. 31 | * Contributors: Martin Guenther 32 | 33 | 1.0.9 (2015-10-16) 34 | ------------------ 35 | 36 | 1.0.8 (2015-10-07) 37 | ------------------ 38 | 39 | 1.0.7 (2015-10-07) 40 | ------------------ 41 | 42 | 1.0.6 (2015-10-06) 43 | ------------------ 44 | 45 | 1.0.5 (2015-06-24) 46 | ------------------ 47 | 48 | 1.0.4 (2015-05-06) 49 | ------------------ 50 | 51 | 1.0.3 (2015-01-29) 52 | ------------------ 53 | 54 | 1.0.2 (2015-01-27) 55 | ------------------ 56 | 57 | 1.0.1 (2014-12-10) 58 | ------------------ 59 | * add me as maintainer to package.xml 60 | * Contributors: Martin Günther 61 | 62 | 1.0.0 (2014-09-03) 63 | ------------------ 64 | * First public release 65 | * Contributors: Ivan Dryanovski, Martin Günther, Davide Tateo, Francisco Vina, Lorenzo Riano 66 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_imu_plugin) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rviz) 5 | 6 | ## This setting causes Qt's "MOC" generation to happen automatically. 7 | set(CMAKE_AUTOMOC ON) 8 | 9 | ## This plugin includes Qt widgets, so we must include Qt. 10 | ## We'll use the version that rviz used so they are compatible. 11 | if(rviz_QT_VERSION VERSION_LESS "5") 12 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 13 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 14 | include(${QT_USE_FILE}) 15 | else() 16 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 17 | find_package(Qt5Widgets REQUIRED) 18 | find_package(Qt5Core REQUIRED) 19 | find_package(Qt5OpenGL REQUIRED) 20 | ## Set the QT_LIBRARIES variable for Qt5, so it can be used below. 21 | set(QT_LIBRARIES Qt5::Widgets) 22 | endif() 23 | 24 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 25 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 26 | add_definitions(-DQT_NO_KEYWORDS) 27 | 28 | catkin_package( 29 | DEPENDS 30 | CATKIN_DEPENDS rviz 31 | INCLUDE_DIRS 32 | LIBRARIES 33 | ) 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | ## Here we specify the list of source files. 41 | ## The generated MOC files are included automatically as headers. 42 | set(SRC_FILES src/imu_display.cpp 43 | src/imu_orientation_visual.cpp 44 | src/imu_axes_visual.cpp 45 | src/imu_acc_visual.cpp) 46 | 47 | ## Build libraries 48 | add_library(${PROJECT_NAME} ${SRC_FILES}) 49 | 50 | ## Link the library with whatever Qt libraries have been defined by 51 | ## the ``find_package(Qt4 ...)`` line above, or by the 52 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 53 | ## catkin has included. 54 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 55 | 56 | install(TARGETS 57 | ${PROJECT_NAME} 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | 63 | install(FILES plugin_description.xml 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 65 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_imu_plugin 3 | 1.0.15 4 | 5 | RVIZ plugin for IMU visualization 6 | 7 | BSD 8 | http://ros.org/wiki/rviz_imu_plugin 9 | Ivan Dryanovski 10 | Martin Günther 11 | Ivan Dryanovski 12 | 13 | catkin 14 | 15 | rviz 16 | rviz 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | Displays the orientation and acceleration components of sensor_msgs/Imu messages. 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: src/doc 3 | -------------------------------------------------------------------------------- /camera_localization/imu_tools/rviz_imu_plugin/rviz_imu_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/camera_localization/imu_tools/rviz_imu_plugin/rviz_imu_plugin.png -------------------------------------------------------------------------------- /camera_localization/localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(localization) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | nav_msgs 7 | roscpp 8 | rospy 9 | sensor_msgs 10 | std_msgs 11 | tf 12 | ) 13 | 14 | catkin_package( 15 | # INCLUDE_DIRS include 16 | # LIBRARIES localization 17 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf 18 | # DEPENDS system_lib 19 | ) 20 | 21 | include_directories( 22 | # include 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | add_executable(realsense_imu_comb_node src/realsense_comb_imu.cpp) 27 | target_link_libraries(realsense_imu_comb_node 28 | ${catkin_LIBRARIES} 29 | ) 30 | 31 | add_executable(tf_pub src/tf_publisher.cpp) 32 | target_link_libraries(tf_pub 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | add_executable(odom_node src/odom_node.cpp) 37 | target_link_libraries(odom_node 38 | ${catkin_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /camera_localization/localization/src/realsense_comb_imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | sensor_msgs::Imu finalImu; 7 | 8 | using namespace std; 9 | 10 | void accelCallback(const sensor_msgs::Imu::ConstPtr& msg) 11 | { 12 | finalImu.header.seq = msg->header.seq; 13 | finalImu.header.frame_id = "camera_link"; 14 | 15 | // finalImu.orientation_covariance << msg->orientation_covariance; 16 | // finalImu.linear_acceleration_covariance << msg->linear_acceleration_covariance; 17 | 18 | finalImu.linear_acceleration.x = msg->linear_acceleration.x; 19 | finalImu.linear_acceleration.y = msg->linear_acceleration.y; 20 | finalImu.linear_acceleration.z = msg->linear_acceleration.z; 21 | 22 | }; 23 | 24 | void gyroCallback(const sensor_msgs::Imu::ConstPtr& msg) 25 | { 26 | // finalImu.angular_velocity_covariance << msg.angular_velocity_covariance; 27 | 28 | finalImu.angular_velocity.x = msg->angular_velocity.x; 29 | finalImu.angular_velocity.y = msg->angular_velocity.y; 30 | finalImu.angular_velocity.z = msg->angular_velocity.z; 31 | }; 32 | 33 | int main(int argc, char** argv) 34 | { 35 | ros::init(argc, argv, "realsense_combined_imu"); 36 | ros::NodeHandle(nh); 37 | ros::Subscriber accel_sub = nh.subscribe("/camera/accel/sample",10,accelCallback); 38 | ros::Subscriber gyro_sub = nh.subscribe("/camera/gyro/sample",10,gyroCallback); 39 | ros::Publisher imu_pub = nh.advertise("/imu/data_raw",100); 40 | 41 | ros::Rate loop_rate(250); 42 | 43 | while(ros::ok()) 44 | { 45 | finalImu.header.stamp = ros::Time::now(); 46 | imu_pub.publish(finalImu); 47 | ros::spinOnce(); 48 | loop_rate.sleep(); 49 | } 50 | 51 | } -------------------------------------------------------------------------------- /octomap_mapping/README.md: -------------------------------------------------------------------------------- 1 | octomap_mapping 2 | =============== 3 | 4 | ROS stack for mapping with OctoMap, contains the octomap_server package. 5 | 6 | The `master` branch tracks the latest (stable) releases. Please send pull requests against the latest development branch, e.g. `indigo-devel`. 7 | 8 | Indigo: [![Build Status](https://travis-ci.org/OctoMap/octomap_mapping.svg?branch=indigo-devel)](https://travis-ci.org/OctoMap/octomap_mapping) 9 | 10 | Kinetic/Melodic: [![Build Status](https://travis-ci.org/OctoMap/octomap_mapping.svg?branch=kinetic-devel)](https://travis-ci.org/OctoMap/octomap_mapping) 11 | 12 | 13 | 14 | Imported from SVN, see https://code.google.com/p/alufr-ros-pkg/ for the previous versions. 15 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_mapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package octomap_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.3 (2019-01-28) 6 | ------------------ 7 | 8 | 0.6.2 (2019-01-27) 9 | ------------------ 10 | * Update maintainer email (Wolfgang Merkt) 11 | * Contributors: Wolfgang Merkt 12 | 13 | 0.6.1 (2016-10-19) 14 | ------------------ 15 | * Adjust maintainer email 16 | * Contributors: Armin Hornung 17 | 18 | 0.6.0 (2016-03-25) 19 | ------------------ 20 | 21 | 0.5.3 (2014-01-09) 22 | ------------------ 23 | 24 | 0.5.2 (2014-01-09) 25 | ------------------ 26 | 27 | 0.5.1 (2013-11-25) 28 | ------------------ 29 | 30 | 0.5.0 (2013-10-24) 31 | ------------------ 32 | * URLs in package.xml 33 | * Catkinization, remove support for arm_navigation 34 | 35 | 0.4.9 (2013-06-27) 36 | ------------------ 37 | 38 | 0.4.8 (2013-01-08) 39 | ------------------ 40 | 41 | 0.4.6 (2013-01-28) 42 | ------------------ 43 | 44 | 0.4.5 (2012-06-18) 45 | ------------------ 46 | 47 | 0.4.4 (2012-04-20) 48 | ------------------ 49 | 50 | 0.4.3 (2012-04-17) 51 | ------------------ 52 | 53 | 0.4.2 (2012-03-16) 54 | ------------------ 55 | 56 | 0.4.1 (2012-02-21 16:50) 57 | ------------------------ 58 | 59 | 0.4.0 (2012-02-21 15:37) 60 | ------------------------ 61 | 62 | 0.3.8 (2012-04-26) 63 | ------------------ 64 | 65 | 0.3.7 (2012-02-22) 66 | ------------------ 67 | 68 | 0.3.6 (2012-01-09) 69 | ------------------ 70 | 71 | 0.3.5 (2011-10-30) 72 | ------------------ 73 | 74 | 0.3.4 (2011-10-12) 75 | ------------------ 76 | 77 | 0.3.3 (2011-08-17 07:41) 78 | ------------------------ 79 | 80 | 0.3.2 (2011-08-09) 81 | ------------------ 82 | 83 | 0.3.1 (2011-07-15) 84 | ------------------ 85 | 86 | 0.3.0 (2011-06-28) 87 | ------------------ 88 | 89 | 0.2.0 (2011-03-16) 90 | ------------------ 91 | 92 | 0.1.2 (2010-11-23) 93 | ------------------ 94 | 95 | 0.1.1 (2010-11-17) 96 | ------------------ 97 | 98 | 0.1.0 (2010-11-16) 99 | ------------------ 100 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(octomap_mapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_mapping 3 | 0.6.3 4 | 5 | Mapping tools to be used with the OctoMap library, implementing a 3D occupancy grid mapping. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | 11 | http://ros.org/wiki/octomap_mapping 12 | https://github.com/OctoMap/octomap_mapping/issues 13 | https://github.com/OctoMap/octomap_mapping 14 | 15 | catkin 16 | octomap_server 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/cfg/OctomapServer.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "octomap_server" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("compress_map", bool_t, 0, "Compresses the map losslessly", True) 9 | gen.add("incremental_2D_projection", bool_t, 0, "Incremental 2D projection", False) 10 | gen.add("filter_speckles", bool_t, 0, "Filter speckle nodes (with no neighbors)", False) 11 | gen.add("max_depth", int_t, 0, "Maximum depth when traversing the octree to send out markers. 16: full depth / max. resolution", 16, 1, 16) 12 | gen.add("pointcloud_min_z", double_t, 0, "Minimum height of points to consider for insertion", -100, -100, 100) 13 | gen.add("pointcloud_max_z", double_t, 0, "Maximum height of points to consider for insertion", 100, -100, 100) 14 | gen.add("occupancy_min_z", double_t, 0, "Minimum height of occupied cells to consider in the final map", -100, -100, 100) 15 | gen.add("occupancy_max_z", double_t, 0, "Maximum height of occupied cells to consider in the final map", 100, -100, 100) 16 | gen.add("sensor_model_max_range", double_t, 0, "Sensor maximum range", -1.0, -1.0, 100) 17 | gen.add("sensor_model_hit", double_t, 0, "Probabilities for hits in the sensor model when dynamically building a map", 0.7, 0.5, 1.0) 18 | gen.add("sensor_model_miss", double_t, 0, "Probabilities for misses in the sensor model when dynamically building a map", 0.4, 0.0, 0.5) 19 | gen.add("sensor_model_min", double_t, 0, "Minimum probability for clamping when dynamically building a map", 0.12, 0.0, 1.0) 20 | gen.add("sensor_model_max", double_t, 0, "Maximum probability for clamping when dynamically building a map", 0.97, 0.0, 1.0) 21 | gen.add("filter_ground", bool_t, 0, "Filter ground plane", False) 22 | gen.add("ground_filter_distance", double_t, 0, "Distance threshold to consider a point as ground", 0.04, 0.001, 1) 23 | gen.add("ground_filter_angle", double_t, 0, "Angular threshold of the detected plane from the horizontal plane to be detected as ground ", 0.15, 0.001, 15) 24 | gen.add("ground_filter_plane_distance", double_t, 0, "Distance threshold from z=0 for a plane to be detected as ground", 0.07, 0.001, 1) 25 | 26 | exit(gen.generate(PACKAGE, "octomap_server_node", "OctomapServer")) 27 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/include/octomap_server/TrackingOctomapServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ 31 | #define OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ 32 | 33 | #include "octomap_server/OctomapServer.h" 34 | 35 | namespace octomap_server { 36 | 37 | class TrackingOctomapServer: public OctomapServer { 38 | public: 39 | TrackingOctomapServer(const std::string& filename = ""); 40 | virtual ~TrackingOctomapServer(); 41 | 42 | void trackCallback(sensor_msgs::PointCloud2Ptr cloud); 43 | void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground); 44 | 45 | protected: 46 | void trackChanges(); 47 | 48 | bool listen_changes; 49 | bool track_changes; 50 | int min_change_pub; 51 | std::string change_id_frame; 52 | ros::Publisher pubFreeChangeSet; 53 | ros::Publisher pubChangeSet; 54 | ros::Subscriber subChangeSet; 55 | ros::Subscriber subFreeChanges; 56 | }; 57 | 58 | } /* namespace octomap */ 59 | #endif /* TRACKINGOCTOMAPSERVER_H_ */ 60 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/launch/octomap_mapping.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/launch/octomap_mapping_nodelet.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/launch/octomap_tracking_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/launch/octomap_tracking_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b octomap_server is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet for running the Octomap server 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_server 3 | 0.6.3 4 | 5 | octomap_server loads a 3D map (as Octree-based OctoMap) and distributes it to other nodes in a compact binary format. It also allows to incrementally build 3D OctoMaps, and provides map saving in the node octomap_saver. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | http://www.ros.org/wiki/octomap_server 11 | https://github.com/OctoMap/octomap_mapping/issues 12 | https://github.com/OctoMap/octomap_mapping 13 | 14 | 15 | 16 | 17 | 18 | catkin 19 | 20 | roscpp 21 | visualization_msgs 22 | sensor_msgs 23 | pcl_ros 24 | pcl_conversions 25 | nav_msgs 26 | std_msgs 27 | std_srvs 28 | octomap 29 | octomap_msgs 30 | octomap_ros 31 | dynamic_reconfigure 32 | nodelet 33 | libpcl-all-dev 34 | 35 | roscpp 36 | visualization_msgs 37 | sensor_msgs 38 | pcl_ros 39 | pcl_conversions 40 | nav_msgs 41 | std_msgs 42 | std_srvs 43 | octomap 44 | octomap_msgs 45 | octomap_ros 46 | dynamic_reconfigure 47 | nodelet 48 | libpcl-all 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/params/default.yaml: -------------------------------------------------------------------------------- 1 | # Empty file to load default parameters. 2 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/scripts/octomap_eraser_cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Clear a region specified by a global axis-aligned bounding box in stored 4 | OctoMap. 5 | 6 | """ 7 | 8 | import sys 9 | from time import sleep 10 | 11 | import roslib 12 | roslib.load_manifest('octomap_server') 13 | from geometry_msgs.msg import Point 14 | import octomap_msgs.srv 15 | import rospy 16 | 17 | 18 | SRV_NAME = '/octomap_server/clear_bbx' 19 | SRV_INTERFACE = octomap_msgs.srv.BoundingBoxQuery 20 | 21 | 22 | if __name__ == '__main__': 23 | min = Point(*[float(x) for x in sys.argv[1:4]]) 24 | max = Point(*[float(x) for x in sys.argv[4:7]]) 25 | 26 | rospy.init_node('octomap_eraser_cli', anonymous=True) 27 | sleep(1) 28 | service = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE) 29 | rospy.loginfo("Connected to %s service." % SRV_NAME) 30 | 31 | service(min, max) 32 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/src/octomap_server_multilayer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009-2013, A. Hornung, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | #include 32 | #include 33 | 34 | #define USAGE "\nUSAGE: octomap_server_multilayer \n" \ 35 | " map.bt: inital octomap 3D map file to read\n" 36 | 37 | using namespace octomap_server; 38 | 39 | int main(int argc, char** argv){ 40 | ros::init(argc, argv, "octomap_server_multilayer"); 41 | std::string mapFilename(""); 42 | 43 | if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ 44 | ROS_ERROR("%s", USAGE); 45 | exit(-1); 46 | } 47 | 48 | 49 | OctomapServerMultilayer server; 50 | ros::spinOnce(); 51 | 52 | if (argc == 2){ 53 | mapFilename = std::string(argv[1]); 54 | if (!server.openFile(mapFilename)){ 55 | ROS_ERROR("Could not open file %s", mapFilename.c_str()); 56 | exit(1); 57 | } 58 | } 59 | 60 | 61 | 62 | 63 | try{ 64 | ros::spin(); 65 | }catch(std::runtime_error& e){ 66 | ROS_ERROR("octomap_server_multilayer exception: %s", e.what()); 67 | return -1; 68 | } 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/src/octomap_server_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * octomap_server_nodelet: A nodelet version of A. Hornung's octomap server 3 | * @author Marcus Liebhardt 4 | * License: BSD 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2012, Marcus Liebhardt, Yujin Robot 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of Yujin Robot nor the names of its 20 | * contributors may be used to endorse or promote products derived from 21 | * this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | 43 | namespace octomap_server 44 | { 45 | 46 | class OctomapServerNodelet : public nodelet::Nodelet 47 | { 48 | public: 49 | virtual void onInit() 50 | { 51 | NODELET_DEBUG("Initializing octomap server nodelet ..."); 52 | ros::NodeHandle& private_nh = this->getPrivateNodeHandle(); 53 | server_.reset(new OctomapServer(private_nh)); 54 | 55 | std::string mapFilename(""); 56 | if (private_nh.getParam("map_file", mapFilename)) { 57 | if (!server_->openFile(mapFilename)){ 58 | NODELET_WARN("Could not open file %s", mapFilename.c_str()); 59 | } 60 | } 61 | } 62 | private: 63 | boost::shared_ptr server_; 64 | }; 65 | 66 | } // namespace 67 | 68 | PLUGINLIB_EXPORT_CLASS(octomap_server::OctomapServerNodelet, nodelet::Nodelet) 69 | -------------------------------------------------------------------------------- /octomap_mapping/octomap_server/src/octomap_tracking_server_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the University of Freiburg nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #define USAGE "\nUSAGE: octomap_tracking_server \n" \ 34 | " map.bt: octomap 3D map file to read\n" 35 | 36 | using namespace octomap_server; 37 | 38 | int main(int argc, char** argv){ 39 | ros::init(argc, argv, "octomap_tracking_server"); 40 | std::string mapFilename(""); 41 | 42 | if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ 43 | ROS_ERROR("%s", USAGE); 44 | exit(-1); 45 | } 46 | 47 | if (argc == 2) 48 | mapFilename = std::string(argv[1]); 49 | 50 | try{ 51 | TrackingOctomapServer ms(mapFilename); 52 | ros::spin(); 53 | }catch(std::runtime_error& e){ 54 | ROS_ERROR("octomap_server exception: %s", e.what()); 55 | return -1; 56 | } 57 | 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /octomap_saver/map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | map_server 3 | 1.14.4 4 | 5 | 6 | map_server provides the map_server ROS Node, which offers map data as a ROS Service. It also provides the map_saver command-line utility, which allows dynamically generated maps to be saved to file. 7 | 8 | 9 | Brian Gerkey, Tony Pratkanis 10 | contradict@gmail.com 11 | David V. Lu!! 12 | Michael Ferguson 13 | Aaron Hoy 14 | http://wiki.ros.org/map_server 15 | BSD 16 | 17 | catkin 18 | 19 | bullet 20 | nav_msgs 21 | roscpp 22 | sdl 23 | sdl-image 24 | tf2 25 | yaml-cpp 26 | 27 | bullet 28 | nav_msgs 29 | roscpp 30 | sdl 31 | sdl-image 32 | tf2 33 | yaml-cpp 34 | 35 | rospy 36 | rostest 37 | rosunit 38 | 39 | -------------------------------------------------------------------------------- /octomap_saver/map_server/src/map_server.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @mainpage 4 | 5 | @htmlinclude manifest.html 6 | 7 | Command-line usage is in the wiki. 8 | */ 9 | 10 | -------------------------------------------------------------------------------- /octomap_saver/map_server/test/consumer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | from __future__ import print_function 36 | 37 | PKG = 'static_map_server' 38 | NAME = 'consumer' 39 | 40 | import sys 41 | import unittest 42 | import time 43 | 44 | import rospy 45 | import rostest 46 | from nav_msgs.srv import GetMap 47 | 48 | 49 | class TestConsumer(unittest.TestCase): 50 | def __init__(self, *args): 51 | super(TestConsumer, self).__init__(*args) 52 | self.success = False 53 | 54 | def callback(self, data): 55 | print(rospy.get_caller_id(), "I heard %s" % data.data) 56 | self.success = data.data and data.data.startswith('hello world') 57 | rospy.signal_shutdown('test done') 58 | 59 | def test_consumer(self): 60 | rospy.wait_for_service('static_map') 61 | mapsrv = rospy.ServiceProxy('static_map', GetMap) 62 | resp = mapsrv() 63 | self.success = True 64 | print(resp) 65 | while not rospy.is_shutdown() and not self.success: # and time.time() < timeout_t: <== timeout_t doesn't exists?? 66 | time.sleep(0.1) 67 | self.assert_(self.success) 68 | rospy.signal_shutdown('test done') 69 | 70 | if __name__ == '__main__': 71 | rostest.rosrun(PKG, NAME, TestConsumer, sys.argv) 72 | -------------------------------------------------------------------------------- /octomap_saver/map_server/test/rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /octomap_saver/map_server/test/test_constants.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | /* This file contains global constants shared among tests */ 33 | 34 | /* Note that these must be changed if the test image changes */ 35 | 36 | #include "test_constants.h" 37 | 38 | const unsigned int g_valid_image_width = 10; 39 | const unsigned int g_valid_image_height = 10; 40 | // Note that the image content is given in row-major order, with the 41 | // lower-left pixel first. This is different from a graphics coordinate 42 | // system, which starts with the upper-left pixel. The loadMapFromFile 43 | // call converts from the latter to the former when it loads the image, and 44 | // we want to compare against the result of that conversion. 45 | const char g_valid_image_content[] = { 46 | 0,0,0,0,0,0,0,0,0,0, 47 | 100,100,100,100,0,0,100,100,100,0, 48 | 100,100,100,100,0,0,100,100,100,0, 49 | 100,0,0,0,0,0,0,0,0,0, 50 | 100,0,0,0,0,0,0,0,0,0, 51 | 100,0,0,0,0,0,100,100,0,0, 52 | 100,0,0,0,0,0,100,100,0,0, 53 | 100,0,0,0,0,0,100,100,0,0, 54 | 100,0,0,0,0,0,100,100,0,0, 55 | 100,0,0,0,0,0,0,0,0,0, 56 | }; 57 | 58 | const char* g_valid_png_file = "test/testmap.png"; 59 | const char* g_valid_bmp_file = "test/testmap.bmp"; 60 | 61 | const float g_valid_image_res = 0.1; 62 | 63 | -------------------------------------------------------------------------------- /octomap_saver/map_server/test/test_constants.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MAP_SERVER_TEST_CONSTANTS_H 30 | #define MAP_SERVER_TEST_CONSTANTS_H 31 | 32 | /* Author: Brian Gerkey */ 33 | 34 | /* This file externs global constants shared among tests */ 35 | 36 | extern const unsigned int g_valid_image_width; 37 | extern const unsigned int g_valid_image_height; 38 | extern const char g_valid_image_content[]; 39 | extern const char* g_valid_png_file; 40 | extern const char* g_valid_bmp_file; 41 | extern const float g_valid_image_res; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /octomap_saver/map_server/test/testmap.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/octomap_saver/map_server/test/testmap.bmp -------------------------------------------------------------------------------- /octomap_saver/map_server/test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/octomap_saver/map_server/test/testmap.png -------------------------------------------------------------------------------- /octomap_saver/map_server/test/testmap.yaml: -------------------------------------------------------------------------------- 1 | image: testmap.png 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /orb_slam_2_ros/.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | .idea/ 3 | cmake-build-debug 4 | KeyFrameTrajectory.txt 5 | Thirdparty/DBoW2/build/ 6 | Thirdparty/DBoW2/lib/ 7 | Thirdparty/g2o/build/ 8 | Thirdparty/g2o/config.h 9 | Thirdparty/g2o/lib/ 10 | Vocabulary/ORBvoc.txt 11 | build/ 12 | *~ 13 | lib/ 14 | .DS_Store 15 | .sync-config.cson 16 | -------------------------------------------------------------------------------- /orb_slam_2_ros/Dependencies.md: -------------------------------------------------------------------------------- 1 | ##List of Known Dependencies 2 | ###ORB-SLAM2 ROS node version 1.0 3 | 4 | In this document we list all the pieces of code included by ORB-SLAM2 and linked libraries which are not property of the authors of ORB-SLAM2. 5 | 6 | 7 | #####Code in **orb_slam2** **src** and **include** folders 8 | 9 | * *ORBextractor.cc*. 10 | This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed. 11 | 12 | * *PnPsolver.h, PnPsolver.cc*. 13 | This is a modified version of the epnp.h and epnp.cc of Vincent Lepetit. 14 | This code can be found in popular BSD licensed computer vision libraries as [OpenCV](https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/epnp.cpp) and [OpenGV](https://github.com/laurentkneip/opengv/blob/master/src/absolute_pose/modules/Epnp.cpp). The original code is FreeBSD. 15 | 16 | * Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*. 17 | The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel. 18 | The code is in the public domain. 19 | 20 | #####Code in Thirdparty folder 21 | 22 | * All code in **DBoW2** folder. 23 | This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed. 24 | 25 | * All code in **g2o** folder. 26 | This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed. 27 | 28 | #####Library dependencies 29 | 30 | * **OpenCV**. 31 | BSD license. 32 | 33 | * **Eigen3**. 34 | For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3. 35 | 36 | * **ROS**. 37 | BSD license. In the manifest.xml the only declared package dependencies are roscpp, tf, sensor_msgs, image_transport, cv_bridge, which are all BSD licensed. 38 | -------------------------------------------------------------------------------- /orb_slam_2_ros/LICENSE.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2. 3 | 4 | For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/raulmur/ORB_SLAM2 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set (dbow2_ROOR_DIR "${PROJECT_SOURCE_DIR}/orb_slam2/Thirdparty/DBoW2") 2 | 3 | set(HDRS_DBOW2 4 | ${dbow2_ROOR_DIR}/DBoW2/BowVector.h 5 | ${dbow2_ROOR_DIR}/DBoW2/FORB.h 6 | ${dbow2_ROOR_DIR}/DBoW2/FClass.h 7 | ${dbow2_ROOR_DIR}/DBoW2/FeatureVector.h 8 | ${dbow2_ROOR_DIR}/DBoW2/ScoringObject.h 9 | ${dbow2_ROOR_DIR}/DBoW2/TemplatedVocabulary.h) 10 | set(SRCS_DBOW2 11 | ${dbow2_ROOR_DIR}/DBoW2/BowVector.cpp 12 | ${dbow2_ROOR_DIR}/DBoW2/FORB.cpp 13 | ${dbow2_ROOR_DIR}/DBoW2/FeatureVector.cpp 14 | ${dbow2_ROOR_DIR}/DBoW2/ScoringObject.cpp) 15 | 16 | set(HDRS_DUTILS 17 | ${dbow2_ROOR_DIR}/DUtils/Random.h 18 | ${dbow2_ROOR_DIR}/DUtils/Timestamp.h) 19 | set(SRCS_DUTILS 20 | ${dbow2_ROOR_DIR}/DUtils/Random.cpp 21 | ${dbow2_ROOR_DIR}/DUtils/Timestamp.cpp) 22 | 23 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${dbow2_ROOR_DIR}/lib) 24 | 25 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 26 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 27 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | /** 75 | * Fills an array with the values from a descriptor 76 | * @param descriptors descriptor 77 | * @param array (out) unsigned char * to store the values of the descriptor 78 | */ 79 | static void toArray8U(const TDescriptor &descriptors, unsigned char * array); 80 | 81 | /** 82 | * Fills an descriptor with the values from an array 83 | * @param descriptors (out) descriptor 84 | * @param array (in) unsigned char * containing the values of the descriptor 85 | */ 86 | static void fromArray8U(TDescriptor &descriptors, unsigned char * array); 87 | 88 | }; 89 | 90 | } // namespace DBoW2 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | 7 | // give a warning if Eigen defaults to row-major matrices. 8 | // We internally assume column-major matrices throughout the code. 9 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 10 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Thirdparty/g2o/license-bsd.txt: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Vocabulary/ORBvoc.txt.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/orb_slam_2_ros/orb_slam2/Vocabulary/ORBvoc.txt.bin -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/Vocabulary/ORBvoc.txt.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whatsuppiyush/slam-octomap/4af13ff0fd5da1f44d335e910fb6bd31e1d8ec7a/orb_slam_2_ros/orb_slam2/Vocabulary/ORBvoc.txt.tar.gz -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/RealSenseD435Mono.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration and distortion parameters (OpenCV) 4 | Camera.fx: 615.6707153320312 5 | Camera.fy: 615.962158203125 6 | Camera.cx: 328.0010681152344 7 | Camera.cy: 241.31031799316406 8 | 9 | # Camera distortion parameters (OpenCV) 10 | Camera.k1: 0.0 11 | Camera.k2: 0.0 12 | Camera.p1: 0.0 13 | Camera.p2: 0.0 14 | Camera.k3: 0.0 15 | 16 | Camera.width: 640 17 | Camera.height: 480 18 | 19 | # Camera frames per second 20 | Camera.fps: 30.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 2000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/RealSenseD435RGBD.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration and distortion parameters (OpenCV) 4 | Camera.fx: 615.6707153320312 5 | Camera.fy: 615.962158203125 6 | Camera.cx: 328.0010681152344 7 | Camera.cy: 241.31031799316406 8 | 9 | # Camera distortion parameters (OpenCV) 10 | Camera.k1: 0.0 11 | Camera.k2: 0.0 12 | Camera.p1: 0.0 13 | Camera.p2: 0.0 14 | Camera.k3: 0.0 15 | 16 | Camera.width: 640 17 | Camera.height: 480 18 | 19 | # IR projector baseline times fx (aprox.) 20 | Camera.bf: 49.833290 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 26 | Camera.RGB: 1 27 | 28 | # Close/Far threshold. Baseline times. 29 | ThDepth: 40.0 30 | 31 | # Deptmap values factor (what pixel value in the depth image corresponds to 1m?) 32 | DepthMapFactor: 1000.0 33 | 34 | #-------------------------------------------------------------------------------------------- 35 | # ORB Parameters 36 | #-------------------------------------------------------------------------------------------- 37 | 38 | # ORB Extractor: Number of features per image 39 | ORBextractor.nFeatures: 1000 40 | 41 | # ORB Extractor: Scale factor between levels in the scale pyramid 42 | ORBextractor.scaleFactor: 1.2 43 | 44 | # ORB Extractor: Number of levels in the scale pyramid 45 | ORBextractor.nLevels: 8 46 | 47 | # ORB Extractor: Fast threshold 48 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 49 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 50 | # You can lower these values if your images have low contrast 51 | ORBextractor.iniThFAST: 20 52 | ORBextractor.minThFAST: 7 53 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/RealSenseR200Mono.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration and distortion parameters (OpenCV) 4 | 5 | Camera.fx: 632.7927856445312 6 | Camera.fy: 626.8605346679688 7 | Camera.cx: 311.43603515625 8 | Camera.cy: 248.0950164794922 9 | 10 | Camera.k1: -0.09097914397716522 11 | Camera.k2: 0.06503549218177795 12 | Camera.p1: 0.000849052332341671 13 | Camera.p2: 0.001785792293958366 14 | Camera.k3: 0.0 15 | 16 | Camera.width: 640 17 | Camera.height: 480 18 | 19 | # Camera frames per second 20 | Camera.fps: 60.0 21 | 22 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 23 | Camera.RGB: 1 24 | 25 | #-------------------------------------------------------------------------------------------- 26 | # ORB Parameters 27 | #-------------------------------------------------------------------------------------------- 28 | 29 | # ORB Extractor: Number of features per image 30 | ORBextractor.nFeatures: 2000 31 | 32 | # ORB Extractor: Scale factor between levels in the scale pyramid 33 | ORBextractor.scaleFactor: 1.2 34 | 35 | # ORB Extractor: Number of levels in the scale pyramid 36 | ORBextractor.nLevels: 8 37 | 38 | # ORB Extractor: Fast threshold 39 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 40 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 41 | # You can lower these values if your images have low contrast 42 | ORBextractor.iniThFAST: 20 43 | ORBextractor.minThFAST: 7 44 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/RealSenseR200RGBD.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration parameters (OpenCV) 4 | Camera.fx: 632.7927 5 | Camera.fy: 626.8605 6 | Camera.cx: 320.0 7 | Camera.cy: 240.0 8 | 9 | # Camera distortion paremeters (OpenCV) -- 10 | Camera.k1: 0.0 11 | Camera.k2: 0.0 12 | Camera.p1: 0.0 13 | Camera.p2: 0.0 14 | Camera.k3: 0.0 15 | 16 | Camera.width: 640 17 | Camera.height: 480 18 | 19 | # IR projector baseline times fx (aprox.) 20 | Camera.bf: 37.2925 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 26 | Camera.RGB: 1 27 | 28 | # Close/Far threshold. Baseline times. 29 | ThDepth: 20.0 30 | 31 | # Deptmap values factor 32 | DepthMapFactor: 1.0 33 | 34 | #-------------------------------------------------------------------------------------------- 35 | # ORB Parameters 36 | #-------------------------------------------------------------------------------------------- 37 | 38 | # ORB Extractor: Number of features per image 39 | ORBextractor.nFeatures: 1000 40 | 41 | # ORB Extractor: Scale factor between levels in the scale pyramid 42 | ORBextractor.scaleFactor: 1.2 43 | 44 | # ORB Extractor: Number of levels in the scale pyramid 45 | ORBextractor.nLevels: 8 46 | 47 | # ORB Extractor: Fast threshold 48 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 49 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 50 | # You can lower these values if your images have low contrast 51 | ORBextractor.iniThFAST: 20 52 | ORBextractor.minThFAST: 7 53 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration and distortion parameters (OpenCV) 4 | Camera.fx: 517.306408 5 | Camera.fy: 516.469215 6 | Camera.cx: 318.643040 7 | Camera.cy: 255.313989 8 | 9 | Camera.k1: 0.262383 10 | Camera.k2: -0.953104 11 | Camera.p1: -0.005358 12 | Camera.p2: 0.002628 13 | Camera.k3: 1.163314 14 | 15 | Camera.width: 640 16 | Camera.height: 480 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # IR projector baseline times fx (aprox.) 22 | Camera.bf: 40.0 23 | 24 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 25 | Camera.RGB: 1 26 | 27 | # Close/Far threshold. Baseline times. 28 | ThDepth: 40.0 29 | 30 | # Deptmap values factor 31 | DepthMapFactor: 5000.0 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # ORB Parameters 35 | #-------------------------------------------------------------------------------------------- 36 | 37 | # ORB Extractor: Number of features per image 38 | ORBextractor.nFeatures: 1000 39 | 40 | # ORB Extractor: Scale factor between levels in the scale pyramid 41 | ORBextractor.scaleFactor: 1.2 42 | 43 | # ORB Extractor: Number of levels in the scale pyramid 44 | ORBextractor.nLevels: 8 45 | 46 | # ORB Extractor: Fast threshold 47 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 48 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 49 | # You can lower these values if your images have low contrast 50 | ORBextractor.iniThFAST: 20 51 | ORBextractor.minThFAST: 7 52 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/mynteye_s_mono.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 332.97713134460906 9 | Camera.fy: 332.97713134460906 10 | Camera.cx: 398.9270935058594 11 | Camera.cy: 252.28187370300293 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 752 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 47.90639384423901 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | #-------------------------------------------------------------------------------------------- 31 | # ORB Parameters 32 | #-------------------------------------------------------------------------------------------- 33 | 34 | # ORB Extractor: Number of features per image 35 | ORBextractor.nFeatures: 1200 36 | 37 | # ORB Extractor: Scale factor between levels in the scale pyramid 38 | ORBextractor.scaleFactor: 1.2 39 | 40 | # ORB Extractor: Number of levels in the scale pyramid 41 | ORBextractor.nLevels: 8 42 | 43 | # ORB Extractor: Fast threshold 44 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 45 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 46 | # You can lower these values if your images have low contrast 47 | ORBextractor.iniThFAST: 20 48 | ORBextractor.minThFAST: 7 49 | 50 | #-------------------------------------------------------------------------------------------- 51 | # Viewer Parameters 52 | #-------------------------------------------------------------------------------------------- 53 | Viewer.KeyFrameSize: 0.05 54 | Viewer.KeyFrameLineWidth: 1 55 | Viewer.GraphLineWidth: 0.9 56 | Viewer.PointSize:2 57 | Viewer.CameraSize: 0.08 58 | Viewer.CameraLineWidth: 3 59 | Viewer.ViewpointX: 0 60 | Viewer.ViewpointY: -0.7 61 | Viewer.ViewpointZ: -1.8 62 | Viewer.ViewpointF: 500 63 | 64 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/config/vimba_stereo.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera calibration and distortion parameters (OpenCV) 4 | Camera.fx: 1208.226731 5 | Camera.fy: 1205.548227 6 | Camera.cx: 305.863428 7 | Camera.cy: 283.394157 8 | 9 | Camera.k1: -0.087742 10 | Camera.k2: 0.374604 11 | Camera.p1: 0.000112 12 | Camera.p2: 0.000612 13 | Camera.k3: 0.000000 14 | 15 | Camera.width: 544 16 | Camera.height: 544 17 | 18 | # Camera frames per second 19 | Camera.fps: 30.0 20 | 21 | # stereo baseline times fx 22 | Camera.bf: 86.14365333630209 23 | 24 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 25 | Camera.RGB: 0 26 | 27 | # Close/Far threshold. Baseline times. 28 | ThDepth: 25 29 | 30 | #-------------------------------------------------------------------------------------------- 31 | # ORB Parameters 32 | #-------------------------------------------------------------------------------------------- 33 | 34 | # ORB Extractor: Number of features per image 35 | ORBextractor.nFeatures: 1200 36 | 37 | # ORB Extractor: Scale factor between levels in the scale pyramid 38 | ORBextractor.scaleFactor: 1.2 39 | 40 | # ORB Extractor: Number of levels in the scale pyramid 41 | ORBextractor.nLevels: 8 42 | 43 | # ORB Extractor: Fast threshold 44 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 45 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 46 | # You can lower these values if your images have low contrast 47 | ORBextractor.iniThFAST: 10 48 | ORBextractor.minThFAST: 4 49 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 28 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 29 | 30 | namespace ORB_SLAM2 31 | { 32 | 33 | class Converter 34 | { 35 | public: 36 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 37 | 38 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 39 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 40 | 41 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 42 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 43 | static cv::Mat toCvMat(const Eigen::Matrix &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 45 | static cv::Mat toCvMat(const Eigen::Matrix &m); 46 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 47 | 48 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 49 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 50 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 51 | 52 | static std::vector toQuaternion(const cv::Mat &M); 53 | }; 54 | 55 | }// namespace ORB_SLAM 56 | 57 | #endif // CONVERTER_H 58 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEDRAWER_H 22 | #define FRAMEDRAWER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace ORB_SLAM2 35 | { 36 | 37 | class Tracking; 38 | 39 | class FrameDrawer 40 | { 41 | public: 42 | FrameDrawer(Map* pMap); 43 | 44 | // Update info from the last processed frame. 45 | void Update(Tracking *pTracker); 46 | 47 | // Draw last processed frame. 48 | cv::Mat DrawFrame(); 49 | 50 | protected: 51 | 52 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 53 | 54 | // Info of the frame to be drawn 55 | cv::Mat mIm; 56 | int N; 57 | vector mvCurrentKeys; 58 | vector mvbMap, mvbVO; 59 | bool mbOnlyTracking; 60 | int mnTracked, mnTrackedVO; 61 | vector mvIniKeys; 62 | vector mvIniMatches; 63 | int mState; 64 | 65 | Map* mpMap; 66 | 67 | std::mutex mMutex; 68 | }; 69 | 70 | } //namespace ORB_SLAM 71 | 72 | #endif // FRAMEDRAWER_H 73 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM2 36 | { 37 | 38 | class KeyFrame; 39 | class Frame; 40 | 41 | 42 | class KeyFrameDatabase 43 | { 44 | public: 45 | 46 | KeyFrameDatabase(const ORBVocabulary &voc); 47 | 48 | void add(KeyFrame* pKF); 49 | 50 | void erase(KeyFrame* pKF); 51 | 52 | void clear(); 53 | 54 | // Loop Detection 55 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 56 | 57 | // Relocalization 58 | std::vector DetectRelocalizationCandidates(Frame* F); 59 | 60 | protected: 61 | 62 | // Associated vocabulary 63 | const ORBVocabulary* mpVoc; 64 | 65 | // Inverted file 66 | std::vector > mvInvertedFile; 67 | 68 | // Mutex 69 | std::mutex mMutex; 70 | }; 71 | 72 | } //namespace ORB_SLAM 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class MapPoint; 36 | class KeyFrame; 37 | 38 | class Map 39 | { 40 | public: 41 | Map(); 42 | 43 | void AddKeyFrame(KeyFrame* pKF); 44 | void AddMapPoint(MapPoint* pMP); 45 | void EraseMapPoint(MapPoint* pMP); 46 | void EraseKeyFrame(KeyFrame* pKF); 47 | void SetReferenceMapPoints(const std::vector &vpMPs); 48 | void InformNewBigChange(); 49 | int GetLastBigChangeIdx(); 50 | 51 | std::vector GetAllKeyFrames(); 52 | std::vector GetAllMapPoints(); 53 | std::vector GetReferenceMapPoints(); 54 | 55 | long unsigned int MapPointsInMap(); 56 | long unsigned KeyFramesInMap(); 57 | 58 | long unsigned int GetMaxKFid(); 59 | 60 | void clear(); 61 | 62 | vector mvpKeyFrameOrigins; 63 | 64 | std::mutex mMutexMapUpdate; 65 | 66 | // This avoid that two points are created simultaneously in separate threads (id conflict) 67 | std::mutex mMutexPointCreation; 68 | 69 | protected: 70 | std::set mspMapPoints; 71 | std::set mspKeyFrames; 72 | 73 | std::vector mvpReferenceMapPoints; 74 | 75 | long unsigned int mnMaxKFid; 76 | 77 | // Index related to a big change in the map (loop closure, global BA) 78 | int mnBigChangeIdx; 79 | 80 | std::mutex mMutexMap; 81 | }; 82 | 83 | } //namespace ORB_SLAM 84 | 85 | #endif // MAP_H 86 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM2 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /orb_slam_2_ros/orb_slam2/include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef OPTIMIZER_H 22 | #define OPTIMIZER_H 23 | 24 | #include "Map.h" 25 | #include "MapPoint.h" 26 | #include "KeyFrame.h" 27 | #include "LoopClosing.h" 28 | #include "Frame.h" 29 | 30 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 31 | 32 | namespace ORB_SLAM2 33 | { 34 | 35 | class LoopClosing; 36 | 37 | class Optimizer 38 | { 39 | public: 40 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 41 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 42 | const bool bRobust = true); 43 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 44 | const unsigned long nLoopKF=0, const bool bRobust = true); 45 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap); 46 | int static PoseOptimization(Frame* pFrame); 47 | 48 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 49 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 50 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 51 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 52 | const map > &LoopConnections, 53 | const bool &bFixScale); 54 | 55 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) 56 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 57 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 58 | }; 59 | 60 | } //namespace ORB_SLAM 61 | 62 | #endif // OPTIMIZER_H 63 | -------------------------------------------------------------------------------- /orb_slam_2_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orb_slam2_ros 4 | 0.0.1 5 | ORB SLAM2 ros implementation. 6 | 7 | Lennart Haller 8 | 9 | GPLv3 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | cv_bridge 16 | image_transport 17 | tf 18 | sensor_msgs 19 | dynamic_reconfigure 20 | roscpp 21 | rospy 22 | std_msgs 23 | cv_bridge 24 | image_transport 25 | tf 26 | sensor_msgs 27 | dynamic_reconfigure 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/config/dynamic_reconfigure.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "orb_slam2_ros" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("localize_only", bool_t, 0, "Disable mapping, localize only", False) 9 | gen.add("reset_map", bool_t, 0, "Erase the map; toggles back to false automatically", False) 10 | gen.add("min_num_kf_in_map", int_t, 0, "Minimum number of key frames in the map for initialization", 5, 1, 50) 11 | gen.add("min_observations_for_ros_map", int_t, 0, "Min num of observations per point for the point cloud; Doesn't change the SLAM itself", 4, 2, 30) 12 | 13 | exit(gen.generate(PACKAGE, "orb_slam2_ros", "dynamic_reconfigure")) 14 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/include/MonoNode.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBSLAM2_ROS_MONONODE_H_ 22 | #define ORBSLAM2_ROS_MONONODE_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "System.h" 37 | #include "Node.h" 38 | 39 | 40 | class MonoNode : public Node 41 | { 42 | public: 43 | MonoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); 44 | ~MonoNode (); 45 | void ImageCallback (const sensor_msgs::ImageConstPtr& msg); 46 | 47 | private: 48 | image_transport::Subscriber image_subscriber; 49 | }; 50 | 51 | #endif //ORBSLAM2_ROS_MONONODE_H_ 52 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/include/Node.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBSLAM2_ROS_NODE_H_ 22 | #define ORBSLAM2_ROS_NODE_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "System.h" 43 | 44 | 45 | 46 | class Node 47 | { 48 | public: 49 | Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); 50 | ~Node (); 51 | 52 | protected: 53 | void Update (); 54 | ORB_SLAM2::System* orb_slam_; 55 | 56 | ros::Time current_frame_time_; 57 | 58 | private: 59 | void PublishMapPoints (std::vector map_points); 60 | void PublishPositionAsTransform (cv::Mat position); 61 | void PublishPositionAsPoseStamped(cv::Mat position); 62 | void PublishRenderedImage (cv::Mat image); 63 | void ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level); 64 | tf::Transform TransformFromMat (cv::Mat position_mat); 65 | sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector map_points); 66 | 67 | dynamic_reconfigure::Server dynamic_param_server_; 68 | 69 | image_transport::Publisher rendered_image_publisher_; 70 | ros::Publisher map_points_publisher_; 71 | ros::Publisher pose_publisher_; 72 | 73 | std::string name_of_node_; 74 | ros::NodeHandle node_handle_; 75 | 76 | std::string map_frame_id_param_; 77 | std::string camera_frame_id_param_; 78 | bool publish_pointcloud_param_; 79 | bool publish_pose_param_; 80 | int min_observations_per_point_; 81 | }; 82 | 83 | #endif //ORBSLAM2_ROS_NODE_H_ 84 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/include/RGBDNode.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBSLAM2_ROS_RGBDODE_H_ 22 | #define ORBSLAM2_ROS_RGBDODE_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "System.h" 40 | #include "Node.h" 41 | 42 | 43 | class RGBDNode : public Node 44 | { 45 | public: 46 | RGBDNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); 47 | ~RGBDNode (); 48 | void ImageCallback (const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 49 | 50 | private: 51 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 52 | message_filters::Subscriber *rgb_subscriber_; 53 | message_filters::Subscriber *depth_subscriber_; 54 | message_filters::Synchronizer *sync_; 55 | }; 56 | 57 | #endif //ORBSLAM2_ROS_RGBDODE_H_ 58 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/include/StereoNode.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "System.h" 16 | #include "Node.h" 17 | 18 | class StereoNode : public Node 19 | { 20 | public: 21 | StereoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); 22 | ~StereoNode (); 23 | void ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight); 24 | 25 | }; 26 | 27 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_d435_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_d435_rgbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_mynteye_s_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_mynteye_s_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_r200_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_r200_rgbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | > 14 | 15 | 16 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/launch/orb_slam2_r200_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/src/MonoNode.cc: -------------------------------------------------------------------------------- 1 | #include "MonoNode.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "Mono"); 6 | ros::start(); 7 | 8 | if(argc != 3) 9 | { 10 | ROS_ERROR ("Path to vocabulary and path to settings need to be set."); 11 | ros::shutdown(); 12 | return 1; 13 | } 14 | 15 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 16 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR); 17 | ros::NodeHandle node_handle; 18 | image_transport::ImageTransport image_transport (node_handle); 19 | 20 | MonoNode node (&SLAM, node_handle, image_transport); 21 | 22 | ros::spin(); 23 | 24 | // Stop all threads 25 | SLAM.Shutdown(); 26 | 27 | // Save camera trajectory 28 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 29 | 30 | ros::shutdown(); 31 | 32 | return 0; 33 | } 34 | 35 | 36 | MonoNode::MonoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { 37 | image_subscriber = image_transport.subscribe ("/camera/image_raw", 1, &MonoNode::ImageCallback, this); 38 | } 39 | 40 | 41 | MonoNode::~MonoNode () { 42 | } 43 | 44 | 45 | void MonoNode::ImageCallback (const sensor_msgs::ImageConstPtr& msg) { 46 | cv_bridge::CvImageConstPtr cv_in_ptr; 47 | try { 48 | cv_in_ptr = cv_bridge::toCvShare(msg); 49 | } catch (cv_bridge::Exception& e) { 50 | ROS_ERROR("cv_bridge exception: %s", e.what()); 51 | return; 52 | } 53 | 54 | current_frame_time_ = msg->header.stamp; 55 | 56 | orb_slam_->TrackMonocular(cv_in_ptr->image,cv_in_ptr->header.stamp.toSec()); 57 | 58 | Update (); 59 | } 60 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/src/RGBDNode.cc: -------------------------------------------------------------------------------- 1 | #include "RGBDNode.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "RGBD"); 6 | ros::start(); 7 | 8 | if(argc != 3) 9 | { 10 | ROS_ERROR ("Path to vocabulary and path to settings need to be set."); 11 | ros::shutdown(); 12 | return 1; 13 | } 14 | 15 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 16 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD); 17 | ros::NodeHandle node_handle; 18 | image_transport::ImageTransport image_transport (node_handle); 19 | 20 | RGBDNode node (&SLAM, node_handle, image_transport); 21 | 22 | ros::spin(); 23 | 24 | // Stop all threads 25 | SLAM.Shutdown(); 26 | 27 | // Save camera trajectory 28 | SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); 29 | 30 | ros::shutdown(); 31 | 32 | return 0; 33 | } 34 | 35 | 36 | RGBDNode::RGBDNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { 37 | rgb_subscriber_ = new message_filters::Subscriber (node_handle, "/camera/rgb/image_raw", 1); 38 | depth_subscriber_ = new message_filters::Subscriber (node_handle, "/camera/depth_registered/image_raw", 1); 39 | 40 | sync_ = new message_filters::Synchronizer (sync_pol(10), *rgb_subscriber_, *depth_subscriber_); 41 | sync_->registerCallback(boost::bind(&RGBDNode::ImageCallback, this, _1, _2)); 42 | } 43 | 44 | 45 | RGBDNode::~RGBDNode () { 46 | delete rgb_subscriber_; 47 | delete depth_subscriber_; 48 | delete sync_; 49 | } 50 | 51 | 52 | void RGBDNode::ImageCallback (const sensor_msgs::ImageConstPtr& msgRGB, const sensor_msgs::ImageConstPtr& msgD) { 53 | // Copy the ros image message to cv::Mat. 54 | cv_bridge::CvImageConstPtr cv_ptrRGB; 55 | try { 56 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 57 | } catch (cv_bridge::Exception& e) { 58 | ROS_ERROR("cv_bridge exception: %s", e.what()); 59 | return; 60 | } 61 | 62 | cv_bridge::CvImageConstPtr cv_ptrD; 63 | try { 64 | cv_ptrD = cv_bridge::toCvShare(msgD); 65 | } catch (cv_bridge::Exception& e) { 66 | ROS_ERROR("cv_bridge exception: %s", e.what()); 67 | return; 68 | } 69 | 70 | current_frame_time_ = msgRGB->header.stamp; 71 | 72 | orb_slam_->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 73 | 74 | Update (); 75 | } 76 | -------------------------------------------------------------------------------- /orb_slam_2_ros/ros/src/StereoNode.cc: -------------------------------------------------------------------------------- 1 | #include "StereoNode.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "Stereo"); 10 | ros::start(); 11 | 12 | if(argc != 3) 13 | { 14 | ROS_ERROR ("Path to vocabulary and path to settings need to be set."); 15 | ros::shutdown(); 16 | return 1; 17 | } 18 | 19 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 20 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO); 21 | ros::NodeHandle node_handle; 22 | image_transport::ImageTransport image_transport (node_handle); 23 | 24 | message_filters::Subscriber left_sub(node_handle, "image_left/image_color_rect", 1); 25 | message_filters::Subscriber right_sub(node_handle, "image_right/image_color_rect", 1); 26 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 27 | message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); 28 | 29 | // initilaize 30 | StereoNode node (&SLAM, node_handle, image_transport); 31 | 32 | // register callbacks 33 | sync.registerCallback(boost::bind(&StereoNode::ImageCallback, &node,_1,_2)); 34 | 35 | ros::spin(); 36 | 37 | // Stop all threads 38 | SLAM.Shutdown(); 39 | 40 | return 0; 41 | } 42 | 43 | 44 | StereoNode::StereoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { 45 | } 46 | 47 | 48 | StereoNode::~StereoNode () { 49 | } 50 | 51 | 52 | void StereoNode::ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft, const sensor_msgs::ImageConstPtr& msgRight) { 53 | cv_bridge::CvImageConstPtr cv_ptrLeft; 54 | try { 55 | cv_ptrLeft = cv_bridge::toCvShare(msgLeft); 56 | } catch (cv_bridge::Exception& e) { 57 | ROS_ERROR("cv_bridge exception: %s", e.what()); 58 | return; 59 | } 60 | 61 | cv_bridge::CvImageConstPtr cv_ptrRight; 62 | try { 63 | cv_ptrRight = cv_bridge::toCvShare(msgRight); 64 | } catch (cv_bridge::Exception& e) { 65 | ROS_ERROR("cv_bridge exception: %s", e.what()); 66 | return; 67 | } 68 | 69 | current_frame_time_ = msgLeft->header.stamp; 70 | 71 | orb_slam_->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); 72 | 73 | Update (); 74 | } 75 | -------------------------------------------------------------------------------- /ptcldfiltering/src/ptcldfilter_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | ros::Publisher ptcld_pub; 12 | 13 | void ptcldCallback(const sensor_msgs::PointCloud2ConstPtr& msg) 14 | { 15 | // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud 16 | pcl::PointCloud::Ptr 17 | cloud(new pcl::PointCloud ()), 18 | cloud_filtered(new pcl::PointCloud ()); 19 | pcl::fromROSMsg (*msg, *cloud); 20 | 21 | pcl::StatisticalOutlierRemoval sor; 22 | sor.setInputCloud (cloud); 23 | sor.setMeanK (50); 24 | sor.setStddevMulThresh (1.0); 25 | sor.filter (*cloud_filtered); 26 | 27 | sensor_msgs::PointCloud2::Ptr 28 | cloud_filtered2(new sensor_msgs::PointCloud2 ()); 29 | pcl::toROSMsg (*cloud_filtered, *cloud_filtered2); 30 | 31 | // Publish the dataSize 32 | ptcld_pub.publish (cloud_filtered2); 33 | } 34 | 35 | int main(int argc,char** argv) 36 | { 37 | ros::init(argc,argv,"ptcldfilter_node"); 38 | ros::NodeHandle(nh); 39 | ros::Rate loop_rate(5); 40 | 41 | ros::Subscriber ptcld_sub = nh.subscribe("/camera/depth/color/points",1,ptcldCallback); 42 | ptcld_pub = nh.advertise("/camera/depth/color/filter/points",10); 43 | while(ros::ok()) 44 | { 45 | ros::spinOnce(); 46 | loop_rate.sleep(); 47 | } 48 | } -------------------------------------------------------------------------------- /quad.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /slam3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam3d) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | ) 8 | catkin_package( 9 | # INCLUDE_DIRS include 10 | # LIBRARIES slam3d 11 | # CATKIN_DEPENDS roscpp rospy 12 | # DEPENDS system_lib 13 | ) 14 | 15 | include_directories( 16 | include 17 | ${catkin_INCLUDE_DIRS} 18 | ) 19 | -------------------------------------------------------------------------------- /slam3d/include/slam3d/slam3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /slam3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | slam3d 4 | 0.0.0 5 | The slam3d package 6 | 7 | 8 | 9 | 10 | ppatel 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | roscpp 55 | rospy 56 | roscpp 57 | rospy 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /timed_roslaunch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(timed_roslaunch) 3 | find_package(catkin REQUIRED) 4 | 5 | catkin_package() 6 | 7 | # Install 8 | install(PROGRAMS 9 | scripts/timed_roslaunch.sh 10 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 11 | ) 12 | 13 | install(DIRECTORY launch/ 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 15 | ) 16 | 17 | install(DIRECTORY test/ 18 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test 19 | ) 20 | 21 | # Test 22 | if (CATKIN_ENABLE_TESTING) 23 | find_package(roslaunch REQUIRED) 24 | roslaunch_add_file_check(launch) 25 | endif() 26 | 27 | -------------------------------------------------------------------------------- /timed_roslaunch/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2016, Masaru Morita 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /timed_roslaunch/README.md: -------------------------------------------------------------------------------- 1 | # timed_roslaunch [![Build Status](https://travis-ci.org/MoriKen254/timed_roslaunch.svg?branch)](https://travis-ci.org/MoriKen254/timed_roslaunch) 2 | 3 | This script can delay the launch of a roslaunch file. 4 | 5 | ## Install 6 | ### apt 7 | ```bash 8 | sudo apt install ros-kinetic-timed-roslaunch 9 | source /opt/ros/kinetic/setup.bash 10 | ``` 11 | 12 | ### source 13 | ```bash 14 | cd ~//src 15 | git clone https://github.com/MoriKen254/timed_roslaunch.git 16 | cd ~/ 17 | catkin_make 18 | source ~//devel/setup.bash 19 | ``` 20 | 21 | ## Usage 22 | This script can delay the launch of a roslaunch file. 23 | Make sure that the file is executable (chmod +x timed_roslaunch.sh) 24 | 25 | ### Run it from command line 26 | 27 | ```bash 28 | rosrun timed_roslaunch timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)] 29 | ``` 30 | 31 | Or 32 | 33 | ```bash 34 | roslaunch timed_roslaunch timed_roslaunch.launch time:=[number of second to delay] pkg:=[rospkg] file:=[roslaunch file] value:=[arguments (optional)] 35 | ``` 36 | 37 | Example: 38 | 39 | ```bash 40 | rosrun timed_roslaunch timed_roslaunch.sh 2 turtlebot_navigation amcl_demo.launch initial_pose_x:=17.0 initial_pose_y:=17.0" 41 | ``` 42 | 43 | ### Run it from another roslaunch file 44 | 45 | ```xml 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | ``` 56 | 57 | Or 58 | 59 | ```xml 60 | 61 | 64 | 65 | ``` 66 | 67 | ## LICENSE 68 | This software is released under the BSD 3-Clause License, see LICENSE.txt. 69 | -------------------------------------------------------------------------------- /timed_roslaunch/launch/example.launch.sample: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /timed_roslaunch/launch/timed_roslaunch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /timed_roslaunch/launch/timed_roslaunch_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /timed_roslaunch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | timed_roslaunch 4 | 0.1.3 5 | 6 | Script to delay the launch of a roslaunch file 7 | 8 | 9 | Masaru Morita 10 | Doi Yusuke 11 | BSD 12 | 13 | https://github.com/MoriKen254/timed_roslaunch.git 14 | https://github.com/MoriKen254/timed_roslaunch/issues 15 | Masaru Morita 16 | 17 | catkin 18 | rospy 19 | roslaunch 20 | 21 | -------------------------------------------------------------------------------- /timed_roslaunch/scripts/timed_roslaunch.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # BSD 3-Clause License 4 | # https://opensource.org/licenses/BSD-3-Clause 5 | # 6 | # Copyright (c) 2016, Masaru Morita 7 | # All rights reserved. 8 | 9 | # -------------------------------------------------------------------- 10 | # Script to delay the launch of a roslaunch file 11 | # 12 | # Usage: sh timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] 13 | # -------------------------------------------------------------------- 14 | 15 | showHelp() { 16 | echo 17 | echo "This script can delay the launch of a roslaunch file" 18 | echo "Make sure that the file is executable (chmod +x timed_roslaunch.sh)" 19 | echo 20 | echo "Run it from command line:" 21 | echo 22 | echo "Usage: ./script/timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 23 | echo "Or: rosrun timed_roslaunch timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 24 | echo "Example: rosrun timed_roslaunch timed_roslaunch.sh 2 turtlebot_navigation amcl_demo.launch initial_pose_x:=17.0 initial_pose_y:=17.0" 25 | echo 26 | echo "Or run it from another roslaunch file:" 27 | echo 28 | echo '' 29 | echo ' ' 30 | echo ' ' 33 | echo ' ' 34 | echo '' 35 | } 36 | 37 | if [ $# -lt 3 -o "$1" = "-h" ]; then 38 | showHelp 39 | else 40 | echo "start wait for $1 seconds" 41 | sleep $1 42 | echo "end wait for $1 seconds" 43 | 44 | shift # The sleep time is droped 45 | echo "now running 'roslaunch $@'" 46 | roslaunch $@ 47 | fi 48 | -------------------------------------------------------------------------------- /timed_roslaunch/test/runtest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /timed_roslaunch/test/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | import rospy 5 | import sys 6 | 7 | rospy.init_node('talker', anonymous=True) 8 | rospy.loginfo("This is a test script.") 9 | # print an argument recieved from test launch file 10 | rospy.loginfo("Argument1: " + sys.argv[1]) 11 | 12 | -------------------------------------------------------------------------------- /timed_roslaunch/test/timed_roslaunch.test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /timed_roslaunch/timed_roslaunch.sh: -------------------------------------------------------------------------------- 1 | cript to delay the launch of a roslaunch file 2 | # 3 | # Koen Lekkerkerker 4 | # Thu 24 Apr 2014 5 | # 6 | # Use: ./timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] 7 | # 8 | 9 | function showHelp(){ 10 | echo 11 | echo "Use: ./timed_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 12 | echo "Or: rosrun [yourpackage] time_roslaunch.sh [number of seconds to delay] [rospkg] [roslaunch file] [arguments (optional)]" 13 | echo "Example: ./timed_roslaunch.sh 2 turtlebot_navigation amcl_demo.launch initial_pose_x:=17.0 initial_pose_y:=17.0" 14 | echo 15 | echo "Script to delay the launch of a roslaunch file" 16 | echo "Place it in the 'script' folder of your catkin package" 17 | echo "and make sure that the file is executable (chmod +x timed_roslaunch.sh)" 18 | echo 19 | echo "Run it from command line is shown above. Or run it from another roslaunch file:" 20 | echo 21 | echo '' 22 | echo ' ' 23 | echo ' ' 26 | echo ' ' 27 | echo '' 28 | } 29 | 30 | if [ "$1" = "-h" ]; then 31 | showHelp 32 | else 33 | echo "start wait for $1 seconds" 34 | sleep $1 35 | echo "end wait for $1 seconds" 36 | shift 37 | echo "now running 'roslaunch $@'" 38 | roslaunch $@ 39 | fi 40 | --------------------------------------------------------------------------------