diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b0f3567 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build +Log/*.png +Log/*.txt +Log/time.pdf diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..31fe073 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fast_lio) + +SET(CMAKE_BUILD_TYPE "Debug") + +ADD_COMPILE_OPTIONS(-std=c++14 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) + +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") + +option(DEPLOY "Deploy on hardware" OFF) +if(DEPLOY) + add_definitions(-DDEPLOY) +endif(DEPLOY) + +find_package(OpenMP QUIET) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +find_package(PythonLibs REQUIRED) +find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + pcl_ros + tf + livox_ros_driver + message_generation + eigen_conversions +) + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.8 REQUIRED) +find_package(OpenCV REQUIRED) + + +message(Eigen: ${EIGEN3_INCLUDE_DIR}) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + include) + +add_message_files( + FILES + Pose6D.msg + States.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs +) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + DEPENDS EIGEN3 PCL OpenCV + INCLUDE_DIRS +) + +# add_executable(imu_process src/IMU_Processing.cpp) +# target_link_libraries(imu_process ${catkin_LIBRARIES} ${PCL_LIBRARIES}) + +add_executable(loam_feat_extract src/feature_extract.cpp) +target_link_libraries(loam_feat_extract ${catkin_LIBRARIES} ${PCL_LIBRARIES}) + +add_executable(loam_laserMapping src/laserMapping.cpp) +target_link_libraries(loam_laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${PYTHON_LIBRARIES}) +target_include_directories(loam_laserMapping PRIVATE ${PYTHON_INCLUDE_DIRS}) +# target_compile_definitions(loam_laserMapping PRIVATE ROOT_DIR="${CMAKE_CURRENT_SOURCE_DIR}") + diff --git a/Log/guide.md b/Log/guide.md new file mode 100644 index 0000000..a620293 --- /dev/null +++ b/Log/guide.md @@ -0,0 +1 @@ +Here saved the debug records which can be drew by the ../tools/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. diff --git a/README.md b/README.md index e6fa49d..200c2fc 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,102 @@ -# FAST_LIO -A computationally efficient and robust LiDAR-inertial odometry (LIO) package +## FAST-LIO +**FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues: +1. Fast iterated Kalman filter for odometry optimization; +2. Automaticaly initialized at most steady environments; +3. Parallel KD-Tree Search to decrease the computation; +4. Robust feature extraction; +5. Surpports for different FOV. + +To know more about the details, please refer to our related paper:) + +**Our related paper**: our related papers are now available on arxiv: + +[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.05957) + +**Our related video**: Commming soon. + +
+ + +
+ +## 1. Prerequisites +### 1.1 **Ubuntu** and **ROS** +Ubuntu >= 18.04. + +ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) + +### 1.2. **PCL && Eigen && openCV** +PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). + +Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). + +OpenCV >= 3.2, Follow [openCV Installation](https://opencv.org/releases/). + +### 1.3. **livox_ros_driver** +Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). + + +## 2. Build +Clone the repository and catkin_make: + +``` + cd ~/catkin_ws/src + git clone https://github.com/XW-HKU/fast_lio.git + cd .. + catkin_make + source devel/setup.bash +``` + +*Remarks:* +- If you want to save the pcd file please add map_file_path in launch file. +## 3. Directly run +### 3.1 For indoor environments and high LiDAR sample rate (20-100hz) +Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then +``` + .... + roslaunch fast_lio mapping_avia.launch + roslaunch livox_ros_driver livox_lidar_msg.launch + +``` + +### 3.2 For outdoor environments +Connect to your PC to Livox Avia LiDAR following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then +``` + .... + roslaunch fast_lio mapping_avia_outdoor.launch + roslaunch livox_ros_driver livox_lidar_msg.launch + +``` +## 4. Rosbag Example +### 4.1 Indoor rosbag (Livox Avia LiDAR) + +
+ +Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWmrwlUD5FlyA-bTr1rakIYx1GxS4xNl/view?usp=sharing) or [avia_indoor_quick_shake_example2](https://drive.google.com/file/d/1wD485CIbzZlNs4z8e20Dv2Q1q-7Gv_AT/view?usp=sharing) and then +``` +roslaunch fast_lio mapping_avia.launch +rosbag play YOUR_DOWNLOADED.bag +``` + +### 4.2 Outdoor rosbag (Livox Avia LiDAR) + +
+ + + +Download [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then +``` +roslaunch fast_lio mapping_avia_outdoor.launch +rosbag play YOUR_DOWNLOADED.bag +``` + +### 4.3 High-rate rosbag (Livox Avia LiDAR sampled at 100Hz) + +Download [high_rate_avia](https://drive.google.com/file/d/1UM6O3PRN3b730ZeuvKKT3yuOLNQuz8Yf/view?usp=sharing) and then +``` +roslaunch fast_lio mapping_avia.launch +rosbag play YOUR_DOWNLOADED.bag +``` + +## 5.Acknowledgments +Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping) and [Loam_Livox](https://github.com/hku-mars/loam_livox). diff --git a/doc/avia_scan.png b/doc/avia_scan.png new file mode 100644 index 0000000..694f9cd Binary files /dev/null and b/doc/avia_scan.png differ diff --git a/doc/avia_scan2.png b/doc/avia_scan2.png new file mode 100644 index 0000000..540ce6c Binary files /dev/null and b/doc/avia_scan2.png differ diff --git a/doc/results/HKU_HW.png b/doc/results/HKU_HW.png new file mode 100644 index 0000000..55061fa Binary files /dev/null and b/doc/results/HKU_HW.png differ diff --git a/doc/results/HKU_LG_Indoor.png b/doc/results/HKU_LG_Indoor.png new file mode 100644 index 0000000..3755445 Binary files /dev/null and b/doc/results/HKU_LG_Indoor.png differ diff --git a/doc/results/HKU_MB_001.png b/doc/results/HKU_MB_001.png new file mode 100644 index 0000000..87f5e20 Binary files /dev/null and b/doc/results/HKU_MB_001.png differ diff --git a/doc/results/HKU_MB_002.png b/doc/results/HKU_MB_002.png new file mode 100644 index 0000000..e891fd1 Binary files /dev/null and b/doc/results/HKU_MB_002.png differ diff --git a/doc/results/HKU_MB_map.png b/doc/results/HKU_MB_map.png new file mode 100644 index 0000000..8f8e7b3 Binary files /dev/null and b/doc/results/HKU_MB_map.png differ diff --git a/doc/results/Screenshot from 2020-10-15 20-33-35.png b/doc/results/Screenshot from 2020-10-15 20-33-35.png new file mode 100644 index 0000000..6914108 Binary files /dev/null and b/doc/results/Screenshot from 2020-10-15 20-33-35.png differ diff --git a/doc/results/Screenshot from 2020-10-15 20-35-46.png b/doc/results/Screenshot from 2020-10-15 20-35-46.png new file mode 100644 index 0000000..9306b61 Binary files /dev/null and b/doc/results/Screenshot from 2020-10-15 20-35-46.png differ diff --git a/doc/results/Screenshot from 2020-10-15 20-37-48.png b/doc/results/Screenshot from 2020-10-15 20-37-48.png new file mode 100644 index 0000000..972f8c8 Binary files /dev/null and b/doc/results/Screenshot from 2020-10-15 20-37-48.png differ diff --git a/doc/results/indoor_loam.png b/doc/results/indoor_loam.png new file mode 100644 index 0000000..4d8888a Binary files /dev/null and b/doc/results/indoor_loam.png differ diff --git a/doc/results/indoor_loam_imu.png b/doc/results/indoor_loam_imu.png new file mode 100644 index 0000000..3de8f7e Binary files /dev/null and b/doc/results/indoor_loam_imu.png differ diff --git a/doc/results/indoor_our.png b/doc/results/indoor_our.png new file mode 100644 index 0000000..236d074 Binary files /dev/null and b/doc/results/indoor_our.png differ diff --git a/doc/results/long_corrid.png b/doc/results/long_corrid.png new file mode 100644 index 0000000..a69887e Binary files /dev/null and b/doc/results/long_corrid.png differ diff --git a/doc/results/uav2_path.png b/doc/results/uav2_path.png new file mode 100644 index 0000000..817c02d Binary files /dev/null and b/doc/results/uav2_path.png differ diff --git a/doc/results/uav2_test_map.png b/doc/results/uav2_test_map.png new file mode 100644 index 0000000..90809fe Binary files /dev/null and b/doc/results/uav2_test_map.png differ diff --git a/doc/results/uav_lg.png b/doc/results/uav_lg.png new file mode 100644 index 0000000..29a0c12 Binary files /dev/null and b/doc/results/uav_lg.png differ diff --git a/doc/results/uav_lg_0.png b/doc/results/uav_lg_0.png new file mode 100644 index 0000000..18bad57 Binary files /dev/null and b/doc/results/uav_lg_0.png differ diff --git a/doc/results/uav_lg_2.png b/doc/results/uav_lg_2.png new file mode 100644 index 0000000..499b50e Binary files /dev/null and b/doc/results/uav_lg_2.png differ diff --git a/include/Exp_mat.h b/include/Exp_mat.h new file mode 100644 index 0000000..84ba4f8 --- /dev/null +++ b/include/Exp_mat.h @@ -0,0 +1,103 @@ +#ifndef EXP_MAT_H +#define EXP_MAT_H + +#include +#include +#include +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T &&theta = std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +// template +// cv::Mat Exp(const T &v1, const T &v2, const T &v3) +// { + +// T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); +// cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); +// if (norm > 0.0000001) +// { +// T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; +// cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); + +// /// Roderigous Tranformation +// return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; +// } +// else +// { +// return Eye3; +// } +// } + +#endif diff --git a/include/common_lib.h b/include/common_lib.h new file mode 100644 index 0000000..933e8ee --- /dev/null +++ b/include/common_lib.h @@ -0,0 +1,140 @@ +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// #define DEBUG_PRINT + +#define PI_M (3.14159265358) +#define G_m_s2 (9.8099) // Gravaty const in GuangDong/China +#define DIM_OF_STATES (18) // Dimension of states (Let Dim(SO(3)) = 3) +#define DIM_OF_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#define CUBE_LEN (6.0) +#define LIDAR_SP_LEN (2) +#define INIT_COV (0.0001) + +#define VEC_FROM_ARRAY(v) v[0],v[1],v[2] +#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] +#define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) + +#define DEBUG_FILE_DIR(name) (std::string(std::string(ROOT_DIR) + "Log/"+ name)) + +typedef fast_lio::Pose6D Pose6D; +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +Eigen::Matrix3d Eye3d(Eigen::Matrix3d::Identity()); +Eigen::Matrix3f Eye3f(Eigen::Matrix3f::Identity()); +Eigen::Vector3d Zero3d(0, 0, 0); +Eigen::Vector3f Zero3f(0, 0, 0); +// Eigen::Vector3d Lidar_offset_to_IMU(0.05512, 0.02226, 0.0297); // Horizon +Eigen::Vector3d Lidar_offset_to_IMU(0.04165, 0.02326, -0.0284); // Avia + +struct MeasureGroup // Lidar data and imu dates for the curent process +{ + MeasureGroup() + { + this->lidar.reset(new PointCloudXYZI()); + }; + double lidar_beg_time; + PointCloudXYZI::Ptr lidar; + std::deque imu; +}; + +struct StatesGroup +{ + StatesGroup() { + this->rot_end = Eigen::Matrix3d::Identity(); + this->pos_end = Zero3d; + this->vel_end = Zero3d; + this->bias_g = Zero3d; + this->bias_a = Zero3d; + this->gravity = Zero3d; + this->cov = Eigen::Matrix::Identity() * INIT_COV; + }; + + StatesGroup& operator+=(const Eigen::Matrix &state_add) + { + this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); + this->pos_end += state_add.block<3,1>(3,0); + this->vel_end += state_add.block<3,1>(6,0); + this->bias_g += state_add.block<3,1>(9,0); + this->bias_a += state_add.block<3,1>(12,0); + this->gravity += state_add.block<3,1>(15,0); + return *this; + }; + + Eigen::Matrix3d rot_end; // the estimated attitude (rotation matrix) at the end lidar point + Eigen::Vector3d pos_end; // the estimated position at the end lidar point (world frame) + Eigen::Vector3d vel_end; // the estimated velocity at the end lidar point (world frame) + Eigen::Vector3d bias_g; // gyroscope bias + Eigen::Vector3d bias_a; // accelerator bias + Eigen::Vector3d gravity; // the estimated gravity acceleration + Eigen::Matrix cov; // states covariance +}; + +template +T rad2deg(T radians) +{ + return radians * 180.0 / PI_M; +} + +template +T deg2rad(T degrees) +{ + return degrees * PI_M / 180.0; +} + +template +auto set_pose6d(const double t, const Eigen::Matrix &a, const Eigen::Matrix &g, \ + const Eigen::Matrix &v, const Eigen::Matrix &p, const Eigen::Matrix &R) +{ + Pose6D rot_kp; + rot_kp.offset_time = t; + for (int i = 0; i < 3; i++) + { + rot_kp.acc[i] = a(i); + rot_kp.gyr[i] = g(i); + rot_kp.vel[i] = v(i); + rot_kp.pos[i] = p(i); + for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); + } + // Eigen::Map(rot_kp.rot, 3,3) = R; + return std::move(rot_kp); +} + +template +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + T x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/include/matplotlibcpp.h b/include/matplotlibcpp.h new file mode 100644 index 0000000..6855445 --- /dev/null +++ b/include/matplotlibcpp.h @@ -0,0 +1,2499 @@ +#pragma once + +// Python headers must be included before any system headers, since +// they define _POSIX_C_SOURCE +#include + +#include +#include +#include +#include +#include +#include +#include +#include // requires c++11 support +#include + +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include + +# ifdef WITH_OPENCV +# include +# endif // WITH_OPENCV + +/* + * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so + * define the ones we need here. + */ +# if CV_MAJOR_VERSION > 3 +# define CV_BGR2RGB cv::COLOR_BGR2RGB +# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA +# endif +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +# define PyInt_FromLong PyLong_FromLong +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject* s_python_function_arrow; + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_fignum_exists; + PyObject *s_python_function_plot; + PyObject *s_python_function_quiver; + PyObject* s_python_function_contour; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_imshow; + PyObject *s_python_function_scatter; + PyObject *s_python_function_boxplot; + PyObject *s_python_function_subplot; + PyObject *s_python_function_subplot2grid; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ginput; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_axvline; + PyObject *s_python_function_axvspan; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_gca; + PyObject *s_python_function_xticks; + PyObject *s_python_function_yticks; + PyObject* s_python_function_margins; + PyObject *s_python_function_tick_params; + PyObject *s_python_function_grid; + PyObject* s_python_function_cla; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_colormap; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + PyObject *s_python_function_text; + PyObject *s_python_function_suptitle; + PyObject *s_python_function_bar; + PyObject *s_python_function_colorbar; + PyObject *s_python_function_subplots_adjust; + + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. + http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + */ + + static _interpreter& get() { + static _interpreter ctx; + return ctx; + } + + PyObject* safe_import(PyObject* module, std::string fname) { + PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); + + if (!fn) + throw std::runtime_error(std::string("Couldn't find required function: ") + fname); + + if (!PyFunction_Check(fn)) + throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); + + return fn; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* cmname = PyString_FromString("matplotlib.cm"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname || !cmname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { + PyErr_Print(); + throw std::runtime_error("Error loading module matplotlib!"); + } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + s_python_colormap = PyImport_Import(cmname); + Py_DECREF(cmname); + if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_arrow = safe_import(pymod, "arrow"); + s_python_function_show = safe_import(pymod, "show"); + s_python_function_close = safe_import(pymod, "close"); + s_python_function_draw = safe_import(pymod, "draw"); + s_python_function_pause = safe_import(pymod, "pause"); + s_python_function_figure = safe_import(pymod, "figure"); + s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); + s_python_function_plot = safe_import(pymod, "plot"); + s_python_function_quiver = safe_import(pymod, "quiver"); + s_python_function_contour = safe_import(pymod, "contour"); + s_python_function_semilogx = safe_import(pymod, "semilogx"); + s_python_function_semilogy = safe_import(pymod, "semilogy"); + s_python_function_loglog = safe_import(pymod, "loglog"); + s_python_function_fill = safe_import(pymod, "fill"); + s_python_function_fill_between = safe_import(pymod, "fill_between"); + s_python_function_hist = safe_import(pymod,"hist"); + s_python_function_scatter = safe_import(pymod,"scatter"); + s_python_function_boxplot = safe_import(pymod,"boxplot"); + s_python_function_subplot = safe_import(pymod, "subplot"); + s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); + s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_ylim = safe_import(pymod, "ylim"); + s_python_function_title = safe_import(pymod, "title"); + s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axvline = safe_import(pymod, "axvline"); + s_python_function_axvspan = safe_import(pymod, "axvspan"); + s_python_function_xlabel = safe_import(pymod, "xlabel"); + s_python_function_ylabel = safe_import(pymod, "ylabel"); + s_python_function_gca = safe_import(pymod, "gca"); + s_python_function_xticks = safe_import(pymod, "xticks"); + s_python_function_yticks = safe_import(pymod, "yticks"); + s_python_function_margins = safe_import(pymod, "margins"); + s_python_function_tick_params = safe_import(pymod, "tick_params"); + s_python_function_grid = safe_import(pymod, "grid"); + s_python_function_xlim = safe_import(pymod, "xlim"); + s_python_function_ion = safe_import(pymod, "ion"); + s_python_function_ginput = safe_import(pymod, "ginput"); + s_python_function_save = safe_import(pylabmod, "savefig"); + s_python_function_annotate = safe_import(pymod,"annotate"); + s_python_function_cla = safe_import(pymod, "cla"); + s_python_function_clf = safe_import(pymod, "clf"); + s_python_function_errorbar = safe_import(pymod, "errorbar"); + s_python_function_tight_layout = safe_import(pymod, "tight_layout"); + s_python_function_stem = safe_import(pymod, "stem"); + s_python_function_xkcd = safe_import(pymod, "xkcd"); + s_python_function_text = safe_import(pymod, "text"); + s_python_function_suptitle = safe_import(pymod, "suptitle"); + s_python_function_bar = safe_import(pymod,"bar"); + s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); + s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); +#ifndef WITHOUT_NUMPY + s_python_function_imshow = safe_import(pymod, "imshow"); +#endif + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +/// Select the backend +/// +/// **NOTE:** This must be called before the first plot command to have +/// any effect. +/// +/// Mainly useful to select the non-interactive 'Agg' backend when running +/// matplotlibcpp in headless mode, for example on a machine with no display. +/// +/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + detail::_interpreter::get(); + + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +namespace detail { + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + +// Sanity checks; comment them out or change the numpy type below if you're compiling on +// a platform where they don't apply +static_assert(sizeof(long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +static_assert(sizeof(unsigned long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; +// TODO: add int, long, etc. + +template +PyObject* get_array(const std::vector& v) +{ + npy_intp vsize = v.size(); + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) { + size_t memsize = v.size()*sizeof(double); + double* dp = static_cast(::malloc(memsize)); + for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); + return varray; + } + + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + + +template +PyObject* get_2darray(const std::vector<::std::vector>& v) +{ + if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); + + npy_intp vsize[2] = {static_cast(v.size()), + static_cast(v[0].size())}; + + PyArrayObject *varray = + (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); + + double *vd_begin = static_cast(PyArray_DATA(varray)); + + for (const ::std::vector &v_row : v) { + if (v_row.size() != static_cast(vsize[1])) + throw std::runtime_error("Missmatched array size"); + std::copy(v_row.begin(), v_row.end(), vd_begin); + vd_begin += vsize[1]; + } + + return reinterpret_cast(varray); +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template +PyObject* get_array(const std::vector& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +// sometimes, for labels and such, we need string arrays +inline PyObject * get_array(const std::vector& strings) +{ + PyObject* list = PyList_New(strings.size()); + for (std::size_t i = 0; i < strings.size(); ++i) { + PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); + } + return list; +} + +// not all matplotlib need 2d arrays, some prefer lists of lists +template +PyObject* get_listlist(const std::vector>& ll) +{ + PyObject* listlist = PyList_New(ll.size()); + for (std::size_t i = 0; i < ll.size(); ++i) { + PyList_SetItem(listlist, i, get_array(ll[i])); + } + return listlist; +} + +} // namespace detail + +/// Plot a line through the given x and y data points.. +/// +/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html +template +bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +// TODO - it should be possible to make this work by implementing +// a non-numpy alternative for `detail::get_2darray()`. +#ifndef WITHOUT_NUMPY +template +void plot_surface(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // We lazily load the modules here the first time this function is called + // because I'm not sure that we can assume "matplotlib installed" implies + // "mpl_toolkits installed" on all platforms, and we don't want to require + // it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); + PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); + if (!plot_surface) throw std::runtime_error("No surface"); + Py_INCREF(plot_surface); + PyObject *res = PyObject_Call(plot_surface, args, kwargs); + if (!res) throw std::runtime_error("failed surface"); + Py_DECREF(plot_surface); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} +#endif // WITHOUT_NUMPY + +template +void plot3(const std::vector &x, + const std::vector &y, + const std::vector &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if (res) Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* y1array = detail::get_array(y1); + PyObject* y2array = detail::get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", + const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { + PyObject* obj_x = PyFloat_FromDouble(x); + PyObject* obj_y = PyFloat_FromDouble(y); + PyObject* obj_end_x = PyFloat_FromDouble(end_x); + PyObject* obj_end_y = PyFloat_FromDouble(end_y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); + PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, obj_x); + PyTuple_SetItem(plot_args, 1, obj_y); + PyTuple_SetItem(plot_args, 2, obj_end_x); + PyTuple_SetItem(plot_args, 3, obj_end_y); + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector& y, long bins=10,std::string color="b", + double alpha=1.0, bool cumulative=false) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +namespace detail { + +inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) +{ + assert(type == NPY_UINT8 || type == NPY_FLOAT); + assert(colors == 1 || colors == 3 || colors == 4); + + detail::_interpreter::get(); + + // construct args + npy_intp dims[3] = { rows, columns, colors }; + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) + throw std::runtime_error("Call to imshow() failed"); + if (out) + *out = res; + else + Py_DECREF(res); +} + +} // namespace detail + +inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); +} + +inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); +} + +#ifdef WITH_OPENCV +void imshow(const cv::Mat &image, const std::map &keywords = {}) +{ + // Convert underlying type of matrix, if needed + cv::Mat image2; + NPY_TYPES npy_type = NPY_UINT8; + switch (image.type() & CV_MAT_DEPTH_MASK) { + case CV_8U: + image2 = image; + break; + case CV_32F: + image2 = image; + npy_type = NPY_FLOAT; + break; + default: + image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); + } + + // If color image, convert from BGR to RGB + switch (image2.channels()) { + case 3: + cv::cvtColor(image2, image2, CV_BGR2RGB); + break; + case 4: + cv::cvtColor(image2, image2, CV_BGRA2RGBA); + } + + detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); +} +#endif // WITH_OPENCV +#endif // WITHOUT_NUMPY + +template +bool scatter(const std::vector& x, + const std::vector& y, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector>& data, + const std::vector& labels = {}, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* listlist = detail::get_listlist(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, listlist); + + PyObject* kwargs = PyDict_New(); + + // kwargs needs the labels, if there are (the correct number of) labels + if (!labels.empty() && labels.size() == data.size()) { + PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); + } + + // take care of the remaining keywords + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector& data, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* vector = detail::get_array(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, vector); + + PyObject* kwargs = PyDict_New(); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & x, + const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject * xarray = detail::get_array(x); + PyObject * yarray = detail::get_array(y); + + PyObject * kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = + keywords.begin(); + it != keywords.end(); + ++it) { + PyDict_SetItemString( + kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject * plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject * res = PyObject_Call( + detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + using T = typename std::remove_reference::type::value_type; + + detail::_interpreter::get(); + + std::vector x; + for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } + + return bar(x, y, ec, ls, lw, keywords); +} + +inline bool subplots_adjust(const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(it->second)); + } + + + PyObject* plot_args = PyTuple_New(0); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool contour(const std::vector& x, const std::vector& y, + const std::vector& z, + const std::map& keywords = {}) { + assert(x.size() == y.size() && x.size() == z.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* zarray = get_array(z); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, zarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) +{ + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, uarray); + PyTuple_SetItem(plot_args, 3, warray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* yerrarray = detail::get_array(yerr); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for(size_t i=0; i +bool plot(const std::vector& y, const std::map& keywords) +{ + std::vector x(y.size()); + for(size_t i=0; i +bool stem(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +template +void text(Numeric x, Numeric y, const std::string& s = "") +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); + if(!res) throw std::runtime_error("Call to text() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) +{ + if (mappable == NULL) + throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); + + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, mappable); + + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); + if(!res) throw std::runtime_error("Call to colorbar() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + + +inline long figure(long number = -1) +{ + detail::_interpreter::get(); + + PyObject *res; + if (number == -1) + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + else { + assert(number > 0); + + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); + Py_DECREF(args); + } + + if(!res) throw std::runtime_error("Call to figure() failed."); + + PyObject* num = PyObject_GetAttrString(res, "number"); + if (!num) throw std::runtime_error("Could not get number attribute of figure object"); + const long figureNumber = PyLong_AsLong(num); + + Py_DECREF(num); + Py_DECREF(res); + + return figureNumber; +} + +inline bool fignum_exists(long number) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); + if(!res) throw std::runtime_error("Call to fignum_exists() failed."); + + bool ret = PyObject_IsTrue(res); + Py_DECREF(res); + Py_DECREF(args); + + return ret; +} + +inline void figure_size(size_t w, size_t h) +{ + detail::_interpreter::get(); + + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +inline void legend(const std::map& keywords) +{ + detail::_interpreter::get(); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(kwargs); + Py_DECREF(res); +} + +template +void ylim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template +void xlim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +template +inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to xticks() failed"); + + Py_DECREF(res); +} + +template +inline void xticks(const std::vector &ticks, const std::map& keywords) +{ + xticks(ticks, {}, keywords); +} + +template +inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to yticks() failed"); + + Py_DECREF(res); +} + +template +inline void yticks(const std::vector &ticks, const std::map& keywords) +{ + yticks(ticks, {}, keywords); +} + +template inline void margins(Numeric margin) +{ + // construct positional args + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template inline void margins(Numeric margin_x, Numeric margin_y) +{ + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline void tick_params(const std::map& keywords, const std::string axis = "both") +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args; + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) throw std::runtime_error("Call to tick_params() failed"); + + Py_DECREF(res); +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) +{ + detail::_interpreter::get(); + + PyObject* shape = PyTuple_New(2); + PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); + PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); + + PyObject* loc = PyTuple_New(2); + PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); + PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); + + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, shape); + PyTuple_SetItem(args, 1, loc); + PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); + PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); + if(!res) throw std::runtime_error("Call to subplot2grid() failed."); + + Py_DECREF(shape); + Py_DECREF(loc); + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pysuptitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); + if(!res) throw std::runtime_error("Call to suptitle() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + detail::_interpreter::get(); + + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + // construct positional args + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + if (it->first == "linewidth" || it->first == "alpha") + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); + else + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void xlabel(const std::string &str, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void set_zlabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); + if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); + Py_INCREF(zlabel); + + PyObject *res = PyObject_Call(zlabel, args, kwargs); + if (!res) throw std::runtime_error("Call to set_zlabel() failed."); + Py_DECREF(zlabel); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +inline void grid(bool flag) +{ + detail::_interpreter::get(); + + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + detail::_interpreter::get(); + + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + detail::_interpreter::get(); + + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template +inline void pause(Numeric interval) +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + detail::_interpreter::get(); + + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + std::cout<<"args:"<> ginput(const int numClicks = 1, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_ginput, args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(args); + if (!res) throw std::runtime_error("Call to ginput() failed."); + + const size_t len = PyList_Size(res); + std::vector> out; + out.reserve(len); + for (size_t i = 0; i < len; i++) { + PyObject *current = PyList_GetItem(res, i); + std::array position; + position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); + position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); + out.push_back(position); + } + Py_DECREF(res); + + return out; +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +// Support for variadic plot() and initializer lists: + +namespace detail { + +template +using is_function = typename std::is_function>>::type; + +template +struct is_callable_impl; + +template +struct is_callable_impl +{ + typedef is_function type; +}; // a non-object is callable iff it is a function + +template +struct is_callable_impl +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + +public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template +struct is_callable +{ + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; +}; + +template +struct plot_impl { }; + +template<> +struct plot_impl +{ + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl +{ + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template +bool plot() { return true; } + +template +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); +} + +inline bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); +} + +inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); +} + +/* + * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting + */ +class Plot +{ +public: + // default initialization with plot label, some data and format + template + Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* kwargs = PyDict_New(); + if(name != "") + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if(res) + { + line= PyList_GetItem(res, 0); + + if(line) + set_data_fct = PyObject_GetAttrString(line,"set_data"); + else + Py_DECREF(line); + Py_DECREF(res); + } + } + + // shorter initialization with name or format only + // basically calls line, = plot([], []) + Plot(const std::string& name = "", const std::string& format = "") + : Plot(name, std::vector(), std::vector(), format) {} + + template + bool update(const std::vector& x, const std::vector& y) { + assert(x.size() == y.size()); + if(set_data_fct) + { + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_CallObject(set_data_fct, plot_args); + if (res) Py_DECREF(res); + return res; + } + return false; + } + + // clears the plot but keep it available + bool clear() { + return update(std::vector(), std::vector()); + } + + // definitely remove this line + void remove() { + if(line) + { + auto remove_fct = PyObject_GetAttrString(line,"remove"); + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(remove_fct, args); + if (res) Py_DECREF(res); + } + decref(); + } + + ~Plot() { + decref(); + } +private: + + void decref() { + if(line) + Py_DECREF(line); + if(set_data_fct) + Py_DECREF(set_data_fct); + } + + + PyObject* line = nullptr; + PyObject* set_data_fct = nullptr; +}; + +} // end namespace matplotlibcpp diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch new file mode 100644 index 0000000..eee4e14 --- /dev/null +++ b/launch/mapping_avia.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mapping_avia_outdoor.launch b/launch/mapping_avia_outdoor.launch new file mode 100644 index 0000000..2c1a223 --- /dev/null +++ b/launch/mapping_avia_outdoor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/replay.launch b/launch/replay.launch new file mode 100644 index 0000000..53eb801 --- /dev/null +++ b/launch/replay.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/msg/Pose6D.msg b/msg/Pose6D.msg new file mode 100644 index 0000000..1ebd1fd --- /dev/null +++ b/msg/Pose6D.msg @@ -0,0 +1,7 @@ +# the preintegrated Lidar states at the time of IMU measurements in a frame +float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point +float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin +float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin +float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin +float64[3] pos # the preintegrated position (global frame) at the Lidar origin +float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin \ No newline at end of file diff --git a/msg/States.msg b/msg/States.msg new file mode 100644 index 0000000..f1b5712 --- /dev/null +++ b/msg/States.msg @@ -0,0 +1,9 @@ +Header header # timestamp of the first lidar in a frame +float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point +float64[] pos_end # the estimated position at the end lidar point (world frame) +float64[] vel_end # the estimated velocity at the end lidar point (world frame) +float64[] bias_gyr # gyroscope bias +float64[] bias_acc # accelerator bias +float64[] gravity # the estimated gravity acceleration +float64[] cov # states covariance +# Pose6D[] IMUpose # 6D pose at each imu measurements \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..98bc774 --- /dev/null +++ b/package.xml @@ -0,0 +1,47 @@ + + + fast_lio + 0.0.0 + + + This is a modified version of LOAM which is original algorithm + is described in the following paper: + J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + + + claydergc + + BSD + + Ji Zhang + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + sensor_msgs + tf + pcl_ros + livox_ros_driver + message_generation + + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + tf + pcl_ros + livox_ros_driver + message_runtime + + rostest + rosbag + + + + diff --git a/rviz_cfg/.gitignore b/rviz_cfg/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz new file mode 100644 index 0000000..c377629 --- /dev/null +++ b/rviz_cfg/loam_livox.rviz @@ -0,0 +1,697 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Axes1 + - /odometry1/odomPath1 + - /mapping1 + - /mapping1/surround1 + - /mapping1/currPoints1 + - /mapping1/PointCloud21 + - /Debug1/PointCloud21 + - /Debug1/PointCloud22 + - /Debug1/PointCloud23 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Odometry1/Odometry2 + - /Odometry1/Odometry2/Shape1 + - /Odometry1/Odometry3 + - /Odometry1/Odometry3/Shape1 + - /Features1 + - /Features1/PointCloud21 + - /Features1/PointCloud22 + - /Features1/PointCloud23 + - /Axes2 + - /Odometry2 + - /Odometry2/Shape1 + - /Marker1 + - /PointStamped1 + Splitter Ratio: 0.5 + Tree Height: 1066 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: currPoints +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Class: rviz/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: gtPathlc + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path_gt + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: gtPathLidar + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path_gt_lidar + Unreliable: false + Value: true + Enabled: false + Name: GT + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.10000000149011612 + Name: odomPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /laser_odom_path + Unreliable: false + Value: true + Enabled: false + Name: odometry + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 10000 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: mappingPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /laser_odom_path + Unreliable: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 4 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: -1.7674484252929688 + Min Value: -8.243647575378418 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 1e+9 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 90.00662994384766 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 0; 0 + Max Intensity: 152 + Min Color: 255; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /Laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 100000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15.00274658203125 + Min Color: 0; 0; 0 + Min Intensity: -0.0028979615308344364 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Spheres + Topic: /laser_cloud_sharp + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.305999994277954 + Min Value: -0.6200000047683716 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 98; 255; 245 + Color Transformer: FlatColor + Decay Time: 1e+6 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 353 + Min Color: 0; 0; 0 + Min Intensity: 2 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /laser_cloud_flat + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 100000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 324 + Min Color: 0; 0; 0 + Min Intensity: 91 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /laser_cloud_less_sharp + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Debug + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1000 + Name: Odometry + Position Tolerance: 0.009999999776482582 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.009999999776482582 + Color: 0; 255; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /re_local_pose + Unreliable: false + Value: true + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Shape: + Alpha: 1 + Axes Length: 0.20000000298023224 + Axes Radius: 0.05000000074505806 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Shape: + Alpha: 1 + Axes Length: 0.20000000298023224 + Axes Radius: 0.05000000074505806 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /re_local_pose + Unreliable: false + Value: true + Enabled: false + Name: Odometry + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 38.0005989074707 + Min Color: 0; 0; 0 + Min Intensity: 1.000100016593933 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 5 + Size (m): 0.20000000298023224 + Style: Points + Topic: /laser_cloud_flat + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 78; 154; 6 + Max Intensity: 155 + Min Color: 78; 154; 6 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05999999865889549 + Style: Flat Squares + Topic: /livox_undistort + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 160 + Min Color: 255; 255; 255 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: /livox_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + Enabled: true + Name: Features + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: aft_mapped + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.20000000298023224 + Axes Radius: 0.05000000074505806 + Color: 25; 255; 255 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.10000000149011612 + Value: Arrow + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: /clicked_point + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 34.710601806640625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 6.608828544616699 + Y: 3.081573247909546 + Z: -3.704350709915161 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -0.17020323872566223 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.0474979877471924 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1383 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000004b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000618000001a800000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df00000052fc0100000002fb0000000800540069006d00650100000000000004df000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000383000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1247 + X: 67 + Y: 27 diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp new file mode 100644 index 0000000..5c7e0fa --- /dev/null +++ b/src/IMU_Processing.hpp @@ -0,0 +1,368 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// *************Preconfiguration + +#define MAX_INI_COUNT (50) + +const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; + +/// *************IMU Process and undistortion +class ImuProcess +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Process(const MeasureGroup &meas, StatesGroup &state, PointCloudXYZI::Ptr pcl_un_); + void Reset(); + void IMU_Initial(const MeasureGroup &meas, StatesGroup &state, int &N); + + // Eigen::Matrix3d Exp(const Eigen::Vector3d &ang_vel, const double &dt); + + void IntegrateGyr(const std::vector &v_imu); + + void UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out); + + ros::NodeHandle nh; + + void Integrate(const sensor_msgs::ImuConstPtr &imu); + void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + + double scale_gravity; + + Eigen::Vector3d angvel_last; + Eigen::Vector3d acc_s_last; + + Eigen::Matrix cov_proc_noise; + + Eigen::Vector3d cov_acc; + Eigen::Vector3d cov_gyr; + + std::ofstream fout; + + private: + /*** Whether is the first frame, init for first frame ***/ + bool b_first_frame_ = true; + bool imu_need_init_ = true; + + int init_iter_num = 1; + Eigen::Vector3d mean_acc; + Eigen::Vector3d mean_gyr; + + /*** Undistorted pointcloud ***/ + PointCloudXYZI::Ptr cur_pcl_un_; + + //// For timestamp usage + sensor_msgs::ImuConstPtr last_imu_; + + /*** For gyroscope integration ***/ + double start_timestamp_; + /// Making sure the equal size: v_imu_ and v_rot_ + std::deque v_imu_; + std::vector v_rot_pcl_; + std::vector IMUpose; +}; + +ImuProcess::ImuProcess() + : b_first_frame_(true), imu_need_init_(true), last_imu_(nullptr), start_timestamp_(-1) +{ + Eigen::Quaterniond q(0, 1, 0, 0); + Eigen::Vector3d t(0, 0, 0); + init_iter_num = 1; + scale_gravity = 1.0; + cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1); + cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1); + mean_acc = Eigen::Vector3d(0, 0, -1.0); + mean_gyr = Eigen::Vector3d(0, 0, 0); + angvel_last = Zero3d; + cov_proc_noise = Eigen::Matrix::Zero(); + // Lidar_offset_to_IMU = Eigen::Vector3d(0.0, 0.0, -0.0); + // fout.open(DEBUG_FILE_DIR("imu.txt"),std::ios::out); +} + +ImuProcess::~ImuProcess() {fout.close();} + +void ImuProcess::Reset() +{ + ROS_WARN("Reset ImuProcess"); + scale_gravity = 1.0; + angvel_last = Zero3d; + cov_proc_noise = Eigen::Matrix::Zero(); + + cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1); + cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1); + mean_acc = Eigen::Vector3d(0, 0, -1.0); + mean_gyr = Eigen::Vector3d(0, 0, 0); + + imu_need_init_ = true; + b_first_frame_ = true; + init_iter_num = 1; + + last_imu_ = nullptr; + + //gyr_int_.Reset(-1, nullptr); + start_timestamp_ = -1; + v_imu_.clear(); + IMUpose.clear(); + + cur_pcl_un_.reset(new PointCloudXYZI()); + fout.close(); + +} + +void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout, int &N) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + Eigen::Vector3d cur_acc, cur_gyr; + + if (b_first_frame_) + { + Reset(); + N = 1; + b_first_frame_ = false; + } + + for (const auto &imu : meas.imu) + { + const auto &imu_acc = imu->linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + + scale_gravity += (cur_acc.norm() - scale_gravity) / N; + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; + + cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); + cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); + + N ++; + } + + state_inout.gravity = - mean_acc /scale_gravity * G_m_s2; + state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(Eigen::Vector3d(0, 0, -1 / scale_gravity))); + state_inout.bias_g = mean_gyr; +} + +void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) +{ + /*** add the imu of the last frame-tail to the of current frame-head ***/ + auto v_imu = meas.imu; + v_imu.push_front(last_imu_); + const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); + const double &imu_end_time = v_imu.back()->header.stamp.toSec(); + const double &pcl_beg_time = meas.lidar_beg_time; + + /*** sort point clouds by offset time ***/ + pcl_out = *(meas.lidar); + std::sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); + const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); + std::cout<<"[ IMU Process ]: Process lidar from "<::Identity()); + Eigen::MatrixXd cov_w(Eigen::Matrix::Zero()); + double dt = 0; + for (auto it_imu = v_imu.begin(); it_imu != (v_imu.end() - 1); it_imu++) + { + auto &&head = *(it_imu); + auto &&tail = *(it_imu + 1); + + angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), + 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), + 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); + acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), + 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), + 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); + + angvel_avr -= state_inout.bias_g; + acc_avr = acc_avr * G_m_s2 / scale_gravity - state_inout.bias_a; + + #ifdef DEBUG_PRINT + // fout<header.stamp.toSec()<<" "<header.stamp.toSec() - head->header.stamp.toSec(); + + /* covariance propagation */ + Eigen::Matrix3d acc_avr_skew; + Eigen::Matrix3d Exp_f = Exp(angvel_avr, dt); + acc_avr_skew<(0,0) = - Exp_f; + F_x.block<3,3>(0,9) = - Eye3d * dt; + // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; + F_x.block<3,3>(3,6) = Eye3d * dt; + F_x.block<3,3>(6,0) = - R_imu * acc_avr_skew * dt; + F_x.block<3,3>(6,12) = - R_imu * dt; + F_x.block<3,3>(6,15) = Eye3d * dt; + + Eigen::Matrix3d cov_acc_diag(Eye3d), cov_gyr_diag(Eye3d); + cov_acc_diag.diagonal() = cov_acc; + cov_gyr_diag.diagonal() = cov_gyr; + cov_w.block<3,3>(0,0).diagonal() = cov_gyr * dt * dt * 10000; + cov_w.block<3,3>(3,3) = R_imu * cov_gyr_diag * R_imu.transpose() * dt * dt * 10000; + cov_w.block<3,3>(6,6) = R_imu * cov_acc_diag * R_imu.transpose() * dt * dt * 10000; + cov_w.block<3,3>(9,9).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias gyro covariance + cov_w.block<3,3>(12,12).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias acc covariance + + state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; + + /* propogation of IMU attitude */ + R_imu = R_imu * Exp_f; + + /* Specific acceleration (global frame) of IMU */ + acc_imu = R_imu * acc_avr + state_inout.gravity; + + /* propogation of IMU */ + pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; + + /* velocity of IMU */ + vel_imu = vel_imu + acc_imu * dt; + + /* save the poses at each IMU measurements */ + angvel_last = angvel_avr; + acc_s_last = acc_imu; + double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; + // std::cout<<"acc "<rot); + acc_imu<acc); + // std::cout<<"head imu acc: "<vel); + pos_imu<pos); + angvel_avr<gyr); + + int i = 0; + for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) + { + dt = it_pcl->curvature / double(1000) - head->offset_time; + + /* Transform to the 'end' frame, using only the rotation + * Note: Compensation direction is INVERSE of Frame's moving direction + * So if we want to compensate a point at timestamp-i to the frame-e + * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ + Eigen::Matrix3d R_i(R_imu * Exp(angvel_avr, dt)); + Eigen::Vector3d T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt + R_i * Lidar_offset_to_IMU - pos_liD_e); + + Eigen::Vector3d P_i(it_pcl->x, it_pcl->y, it_pcl->z); + Eigen::Vector3d P_compensate = state_inout.rot_end.transpose() * (R_i * P_i + T_ei); + + /// save Undistorted points and their rotation + it_pcl->x = P_compensate(0); + it_pcl->y = P_compensate(1); + it_pcl->z = P_compensate(2); + + if (it_pcl == pcl_out.points.begin()) break; + } + } +} + +void ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) +{ + double t1,t2,t3; + t1 = omp_get_wtime(); + + if(meas.imu.empty()) {std::cout<<"no imu data"< MAX_INI_COUNT) + { + imu_need_init_ = false; + // std::cout<<"mean acc: "<("/livox_undistort", 100); + // sensor_msgs::PointCloud2 pcl_out_msg; + // pcl::toROSMsg(*cur_pcl_un_, pcl_out_msg); + // pcl_out_msg.header.stamp = ros::Time().fromSec(meas.lidar_beg_time); + // pcl_out_msg.header.frame_id = "/livox"; + // pub_UndistortPcl.publish(pcl_out_msg); + // } + + /// Record last measurements + last_imu_ = meas.imu.back(); + + t3 = omp_get_wtime(); + + std::cout<<"[ IMU Process ]: Time: "< +#include +#include +#include + +typedef pcl::PointXYZINormal PointType; +using namespace std; +ros::Publisher pub_full, pub_surf, pub_corn; + +enum LID_TYPE{MID, HORIZON, VELO16, OUST64}; + +enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; +enum Surround{Prev, Next}; +enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; + +struct orgtype +{ + double range; // sqrt(x*x+y*y) + double dista; // 该点与后一个点的距离平方 + double angle[2]; // 前(后)一个点、该点、原点所成角度 + double intersect; // 前后点与该点的夹角 + E_jump edj[2]; // 每个点前后点的关系 + // Surround nor_dir; + Feature ftype; + orgtype() + { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } +}; + +// const int hor_line = 6; +const double rad2deg = 180*M_1_PI; + +int lidar_type; +double blind, inf_bound; +int N_SCANS; +int group_size; +double disA, disB; +double limit_maxmid, limit_midmin, limit_maxmin; +double p2l_ratio; +double jump_up_limit, jump_down_limit; +double cos160; +double edgea, edgeb; +double smallp_intersect, smallp_ratio; +int point_filter_num; + +void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); +void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); +void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); +void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); +void give_feature(pcl::PointCloud &pl, vector &types, pcl::PointCloud &pl_corn, pcl::PointCloud &pl_surf); +void pub_func(pcl::PointCloud &pl, ros::Publisher pub, const ros::Time &ct); +int plane_judge(const pcl::PointCloud &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); +bool small_plane(const pcl::PointCloud &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); +bool edge_jump_judge(const pcl::PointCloud &pl, vector &types, uint i, Surround nor_dir); + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "feature_extract"); + ros::NodeHandle n; + + // lidar_type = MID; + // blind = 0.5; + // inf_bound = 10; + // // N_SCANS = 1; + // group_size = 8; + // disA = 0.01; disB = 0.1; + // p2l_ratio = 400; + // limit_maxmid = 9; + // limit_midmin = 16; + // // limit_maxmin; + // jump_up_limit = cos(175.0/180*M_PI); + // jump_down_limit = cos(5.0/180*M_PI); + // cos160 = cos(160.0/180*M_PI); + // edgea = 3; // 2 + // edgeb = 0.05; // 0.1 + // smallp_intersect = cos(170.0/180*M_PI); + // smallp_ratio = 1.3; + // point_filter_num = 4; + + n.param("lidar_type", lidar_type, 0); + n.param("blind", blind, 0.5); + n.param("inf_bound", inf_bound, 10); + n.param("N_SCANS", N_SCANS, 1); + n.param("group_size", group_size, 8); + n.param("disA", disA, 0.01); + n.param("disB", disB, 0.1); + n.param("p2l_ratio", p2l_ratio, 400); + n.param("limit_maxmid", limit_maxmid, 9); + n.param("limit_midmin", limit_midmin, 16); + n.param("limit_maxmin", limit_maxmin, 3.24); + n.param("jump_up_limit", jump_up_limit, 175.0); + n.param("jump_down_limit", jump_down_limit, 5.0); + n.param("cos160", cos160, 160.0); + n.param("edgea", edgea, 3); + n.param("edgeb", edgeb, 0.05); + n.param("smallp_intersect", smallp_intersect, 170.0); + n.param("smallp_ratio", smallp_ratio, 1.2); + n.param("point_filter_num", point_filter_num, 4); + + jump_up_limit = cos(jump_up_limit/180*M_PI); + jump_down_limit = cos(jump_down_limit/180*M_PI); + cos160 = cos(cos160/180*M_PI); + smallp_intersect = cos(smallp_intersect/180*M_PI); + + // lidar_type = HORIZON; + // blind = 0.5; + // inf_bound = 10; + // N_SCANS = 6; + // group_size = 8; + // disA = 0.01; disB = 0.1; + // p2l_ratio = 225; + // limit_maxmid = 6.25; + // limit_midmin = 6.25; + // // limit_maxmin; + // jump_up_limit = cos(170.0/180*M_PI); + // jump_down_limit = cos(8.0/180*M_PI); + // cos160 = cos(160.0/180*M_PI); + // edgea = 2; + // edgeb = 0.1; + // smallp_intersect = cos(170.0/180*M_PI); + // smallp_ratio = 1.4; + + // lidar_type = VELO16; + // blind = 0.5; + // inf_bound = 10; + // N_SCANS = 16; + // group_size = 9; + // disA = 0.015; disB = 0.3; + // p2l_ratio = 400; + // // limit_maxmid = 9; + // // limit_midmin = 16; + // limit_maxmin = 3.24; + // jump_up_limit = cos(170.0/180*M_PI); + // jump_down_limit = cos(8.0/180*M_PI); + // cos160 = cos(160.0/180*M_PI); + // edgea = 2; // 2 + // edgeb = 0.1; // 0.1 + // smallp_intersect = cos(170.0/180*M_PI); + // smallp_ratio = 1.4; + + // lidar_type = OUST64; // lidar的种类 + // blind = 0.5; // lidar的盲区（非平方） + // inf_bound = 10; // 无穷远点最近点的距离限制（非平方） + // N_SCANS = 64; // 6, 16, 64 + // group_size = 9; // 7->8 9 + // disA = 0.015; // 0.015 // Ax+B + // disB = 0.3; // 0.3 + // p2l_ratio = 196; // 225 196 + // // limit_maxmid = 9; // 6.25 + // // limit_midmin = 16; // 6.25 + // limit_maxmin = 6.25; // 6.25 + // jump_up_limit = cos(170.0/180*M_PI); + // jump_down_limit = cos(8.0/180*M_PI); + // cos160 = cos(160.0/180*M_PI); + // edgea = 2.5; // 2 + // edgeb = 0.2; // 0.1 + // smallp_intersect = cos(170.0/180*M_PI); + // smallp_ratio = 1.4; + + + ros::Subscriber sub_points; + + + switch(lidar_type) + { + case MID: + printf("MID40-70\n"); + sub_points = n.subscribe("/livox/lidar", 1000, mid_handler); + // sub_points = n.subscribe("/livox/lidar_1LVDG1S006J5GZ3", 1000, mid_handler); + break; + + case HORIZON: + printf("HORIZON_MID70pro\n"); + sub_points = n.subscribe("/livox/lidar", 1000, horizon_handler); + break; + + case VELO16: + printf("VELO16\n"); + sub_points = n.subscribe("/velodyne_points", 1000, velo16_handler); + break; + + case OUST64: + printf("OUST64\n"); + sub_points = n.subscribe("/os1_cloud_node/points", 1000, oust64_handler); + break; + + default: + printf("Lidar type is wrong.\n"); + exit(0); + break; + } + + pub_full = n.advertise("/livox_cloud", 20); + pub_surf = n.advertise("/laser_cloud_flat", 20); + pub_corn = n.advertise("/laser_cloud_sharp", 20); + + ros::spin(); + return 0; +} + + +double vx, vy, vz; +void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pcl::PointCloud pl; + pcl::fromROSMsg(*msg, pl); + + pcl::PointCloud pl_corn, pl_surf; + vector types; + uint plsize = pl.size()-1; + pl_corn.reserve(plsize); pl_surf.reserve(plsize); + types.resize(plsize+1); + + for(uint i=0; i> a; + // if(a == 0) + // { + // exit(0); + // } + +} + +void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) +{ + double t1 = omp_get_wtime(); + vector> pl_buff(N_SCANS); + vector> typess(N_SCANS); + pcl::PointCloud pl_full, pl_corn, pl_surf; + + uint plsize = msg->point_num; + pl_corn.reserve(plsize); pl_surf.reserve(plsize); + pl_full.resize(plsize); + + for(int i=0; ipoints[i].line < N_SCANS) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + + for(int j=0; j &pl = pl_buff[j]; + vector &types = typess[j]; + plsize = pl.size(); + types.resize(plsize); + plsize--; + for(uint i=0; itimebase); + pub_func(pl_full, pub_full, ct); + pub_func(pl_surf, pub_surf, ct); + pub_func(pl_corn, pub_corn, ct); + std::cout<<"[~~~~~~~~~~~~ Feature Extract ]: time: "<< omp_get_wtime() - t1<<" "<header.stamp.toSec()< pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + + pcl::PointCloud pl_corn, pl_surf, pl_full; + vector> pl_buff(N_SCANS); + vector> typess(N_SCANS); + + uint plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + pl_full.reserve(plsize); + + for(int i=0; i blind) + { + if(stat == 0) + { + stat = 1; + idx++; + } + + double ang = atan(ap.z / leng)*rad2deg; + scanID = int((ang + 15) / 2 + 0.5); + if(scanID>=N_SCANS || scanID <0) + { + continue; + } + pl_buff[scanID][idx] = ap; + typess[scanID][idx].range = leng; + pl_full.push_back(ap); + } + else + { + stat = 0; + } + } + idx++; + + + for(int j=0; j &pl = pl_buff[j]; + vector &types = typess[j]; + pl.erase(pl.begin()+idx, pl.end()); + types.erase(types.begin()+idx, types.end()); + plsize = idx - 1; + for(uint i=0; i pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + + vector> pl_buff(N_SCANS); + vector> typess(N_SCANS); + pcl::PointCloud pl_corn, pl_surf; + + uint plsize = pl_orig.size(); + + pl_corn.reserve(plsize); pl_surf.reserve(plsize); + for(int i=0; i &pl = pl_buff[j]; + vector &types = typess[j]; + plsize = pl.size() - 1; + types.resize(plsize+1); + for(uint i=0; i &pl, vector &types, pcl::PointCloud &pl_corn, pcl::PointCloud &pl_surf) +{ + uint plsize = pl.size(); + uint plsize2; + if(plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + while(types[head].range < blind) + { + head++; + } + + // 平面点检测 + plsize2 = plsize - group_size; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex, i2; + uint last_i = 0; uint last_i_nex = 0; + // 0:上次状态无用 1:上次为平面组 + int last_state = 0; + int plane_type; + + for(uint i=head; i0.5) + if(last_state==1 && last_direct.norm()>0.1) + { + double mod = last_direct.transpose() * curr_direct; + if(mod>-0.707 && mod<0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + else if(plane_type == 0) + { + if(last_state == 1) + { + uint i_nex_tem; // 临时变量 + uint j; + for(j=last_i+1; j<=last_i_nex; j++) + { + uint i_nex_tem2 = i_nex_tem; + Eigen::Vector3d curr_direct2; // curr_direct临时变量 + + uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + if(ttem != 1) + { + i_nex_tem = i_nex_tem2; + break; + } + curr_direct = curr_direct2; + } + + if(j == last_i+1) + { + last_state = 0; + } + else + { + for(uint k=last_i_nex; k<=i_nex_tem; k++) + { + if(k != i_nex_tem) + { + types[k].ftype = Real_Plane; + } + else + { + types[k].ftype = Poss_Plane; + } + } + i = i_nex_tem-1; + i_nex = i_nex_tem; + i2 = j-1; + last_state = 1; + } + + } + } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + + } + + plsize2 = plsize - 3; + // for(uint i=head+3; i=Real_Plane) + // { + // continue; + // } + + // if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + // { + // continue; + // } + + // Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + // Eigen::Vector3d vecs[2]; + + // for(int j=0; j<2; j++) + // { + // int m = -1; + // if(j == 1) + // { + // m = 1; + // } + + // if(types[i+m].range < blind) + // { + // if(types[i].range > inf_bound) + // { + // types[i].edj[j] = Nr_inf; + // } + // else + // { + // types[i].edj[j] = Nr_blind; + // } + // continue; + // } + + // vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + // vecs[j] = vecs[j] - vec_a; + + // types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + // if(types[i].angle[j] < jump_up_limit) + // { + // types[i].edj[j] = Nr_180; + // } + // else if(types[i].angle[j] > jump_down_limit) + // { + // types[i].edj[j] = Nr_zero; + // } + // } + + // types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + // if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + // { + // if(types[i].intersect > cos160) + // { + // if(edge_jump_judge(pl, types, i, Prev)) + // { + // types[i].ftype = Edge_Jump; + // } + // } + // } + // else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + // { + // if(types[i].intersect > cos160) + // { + // if(edge_jump_judge(pl, types, i, Next)) + // { + // types[i].ftype = Edge_Jump; + // } + // } + // } + // else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + // { + // if(edge_jump_judge(pl, types, i, Prev)) + // { + // types[i].ftype = Edge_Jump; + // } + // } + // else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + // { + // if(edge_jump_judge(pl, types, i, Next)) + // { + // types[i].ftype = Edge_Jump; + // } + + // } + // else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + // { + // if(types[i].ftype == Nor) + // { + // types[i].ftype = Wire; + // } + // } + // } + + plsize2 = plsize-1; + double ratio; + for(uint i=head+1; i types[i].dista) + { + ratio = types[i-1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i-1].dista; + } + + if(types[i].intersect &pl, ros::Publisher pub, const ros::Time &ct) +{ + pl.height = 1; pl.width = pl.size(); + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(pl, output); + output.header.frame_id = "livox"; + output.header.stamp = ct; + pub.publish(output); +} + +int plane_judge(const pcl::PointCloud &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA*types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for(i_nex=i_cur; i_nex= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i_cur+1; j leng_wid) + { + leng_wid = lw; + } + } + + + if((two_dis*two_dis/leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize-2]; + if(dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + + +bool edge_jump_judge(const pcl::PointCloud &pl, vector &types, uint i, Surround nor_dir) +{ + if(nor_dir == 0) + { + if(types[i-1].rangeedgea*d2 || (d1-d2)>edgeb) + { + return false; + } + + return true; +} + + +int plane_judge1(const pcl::PointCloud &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = 0.01*pl[i].x + 0.1; + group_dis = group_dis*group_dis; + + double two_dis; + vector disarr; + disarr.reserve(128); + + for(i_nex=i; i_nex= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i+1; j leng_wid) + { + leng_wid = lw; + } + } + + // if((two_dis*two_dis/leng_wid) <= 225) + if((two_dis*two_dis/leng_wid) <= 400) + { + curr_direct = Eigen::Vector3d::Zero(); + return 0; + } + + // 寻找中位数(不是准确的中位数，有点偏差) + for(uint32_t j=0; j=9 || dismid_min>=16) + { + curr_direct = Eigen::Vector3d::Zero(); + return 0; + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool edge_jump_judge1(const pcl::PointCloud &pl, vector &types, uint i, Surround nor_dir) +{ + // if(nor_dir == 0) + // { + // if(pl[i-1].x0.05) + { + if(d1>3*d2 || (d1-d2)>0.05) + { + return false; + } + } + return true; +} + + + diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp new file mode 100644 index 0000000..474039e --- /dev/null +++ b/src/laserMapping.cpp @@ -0,0 +1,1080 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +// Modifier: Livox dev@livoxtech.com + +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "IMU_Processing.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#ifndef DEPLOY +#include "matplotlibcpp.h" +namespace plt = matplotlibcpp; +#endif + +#define INIT_TIME (1.0) +#define LASER_POINT_COV (0.0015) +#define NUM_MATCH_POINTS (5) + +std::string root_dir = ROOT_DIR; + +int iterCount = 0; +int NUM_MAX_ITERATIONS = 0; +int laserCloudCenWidth = 20; +int laserCloudCenHeight = 10; +int laserCloudCenDepth = 20; +int laserCloudValidInd[250]; +int laserCloudSurroundInd[250]; +int laserCloudValidNum = 0; +int laserCloudSurroundNum = 0; + +const int laserCloudWidth = 42; +const int laserCloudHeight = 22; +const int laserCloudDepth = 42; +const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851 + +/// IMU relative variables +std::mutex mtx_buffer; +std::condition_variable sig_buffer; +bool lidar_pushed = false; +bool b_exit = false; +bool b_reset = false; +bool b_first = true; + +/// Buffers for measurements +double cube_len = 0.0; +double lidar_end_time = 0.0; +double last_timestamp_lidar = -1; +double last_timestamp_imu = -1; +double HALF_FOV_COS = 0.0; + +std::deque lidar_buffer; +std::deque imu_buffer; + +//surf feature in map +PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); + +std::deque< fast_lio::States > rot_kp_imu_buff; + +//all points +PointCloudXYZI::Ptr laserCloudFullRes2(new PointCloudXYZI()); +PointCloudXYZI::Ptr featsArray[laserCloudNum]; +pcl::PointCloud::Ptr laserCloudFullResColor(new pcl::PointCloud()); +pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); + +//estimator inputs and output; +MeasureGroup Measures; +StatesGroup state; + +void SigHandle(int sig) +{ + b_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); +} + +//project lidar frame to world +void pointBodyToWorld(PointType const * const pi, PointType * const po) +{ + Eigen::Vector3d p_body(pi->x, pi->y, pi->z); + Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +void RGBpointBodyToWorld(PointType const * const pi, pcl::PointXYZRGB * const po) +{ + Eigen::Vector3d p_body(pi->x, pi->y, pi->z); + Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + //po->intensity = pi->intensity; + + float intensity = pi->intensity; + intensity = intensity - std::floor(intensity); + + int reflection_map = intensity*10000; + + //std::cout<<"DEBUG reflection_map "<r = 0; + po->g = green & 0xff; + po->b = 0xff; + } + else if (reflection_map < 90) + { + int blue = (((90 - reflection_map) * 255) / 60); + po->r = 0x0; + po->g = 0xff; + po->b = blue & 0xff; + } + else if (reflection_map < 150) + { + int red = ((reflection_map-90) * 255 / 60); + po->r = red & 0xff; + po->g = 0xff; + po->b = 0x0; + } + else + { + int green = (((255-reflection_map) * 255) / (255-150)); + po->r = 0xff; + po->g = green & 0xff; + po->b = 0; + } +} + +void lasermap_fov_segment() +{ + laserCloudValidNum = 0; + laserCloudSurroundNum = 0; + PointType pointOnYAxis; + pointOnYAxis.x = LIDAR_SP_LEN; + pointOnYAxis.y = 0.0; + pointOnYAxis.z = 0.0; + pointBodyToWorld(&pointOnYAxis, &pointOnYAxis); + // std::cout<<"special point:"<= 1; i--) { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + } + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + centerCubeI++; + laserCloudCenWidth++; + } + + while (centerCubeI >= laserCloudWidth - 3) { + for (int j = 0; j < laserCloudHeight; j++) { + for (int k = 0; k < laserCloudDepth; k++) { + int i = 0; + PointCloudXYZI::Ptr laserCloudCubeSurfPointer = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + + for (; i < laserCloudWidth - 1; i++) + { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + } + + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeI--; + laserCloudCenWidth--; + } + + while (centerCubeJ < 3) { + for (int i = 0; i < laserCloudWidth; i++) { + for (int k = 0; k < laserCloudDepth; k++) { + int j = laserCloudHeight - 1; + PointCloudXYZI::Ptr laserCloudCubeSurfPointer = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + + for (; j >= 1; j--) { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k]; + } + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeJ++; + laserCloudCenHeight++; + } + + while (centerCubeJ >= laserCloudHeight - 3) { + for (int i = 0; i < laserCloudWidth; i++) { + for (int k = 0; k < laserCloudDepth; k++) { + int j = 0; + PointCloudXYZI::Ptr laserCloudCubeSurfPointer = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + + for (; j < laserCloudHeight - 1; j++) { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k]; + } + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeJ--; + laserCloudCenHeight--; + } + + while (centerCubeK < 3) { + for (int i = 0; i < laserCloudWidth; i++) { + for (int j = 0; j < laserCloudHeight; j++) { + int k = laserCloudDepth - 1; + PointCloudXYZI::Ptr laserCloudCubeSurfPointer = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + + for (; k >= 1; k--) { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)]; + } + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeK++; + laserCloudCenDepth++; + } + + while (centerCubeK >= laserCloudDepth - 3) + { + for (int i = 0; i < laserCloudWidth; i++) + { + for (int j = 0; j < laserCloudHeight; j++) + { + int k = 0; + PointCloudXYZI::Ptr laserCloudCubeSurfPointer = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + + for (; k < laserCloudDepth - 1; k++) + { + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)]; + } + + featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeK--; + laserCloudCenDepth--; + } + + for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) + { + for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) + { + for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) + { + if (i >= 0 && i < laserCloudWidth && + j >= 0 && j < laserCloudHeight && + k >= 0 && k < laserCloudDepth) + { + + float centerX = cube_len * (i - laserCloudCenWidth); + float centerY = cube_len * (j - laserCloudCenHeight); + float centerZ = cube_len * (k - laserCloudCenDepth); + + float check1, check2; + float squaredSide1, squaredSide2; + float ang_cos = 1; + + bool isInLaserFOV = false; + + for (int ii = -1; ii <= 1; ii += 2) + { + for (int jj = -1; jj <= 1; jj += 2) + { + for (int kk = -1; (kk <= 1) && (!isInLaserFOV); kk += 2) + { + + float cornerX = centerX + 0.5 * cube_len * ii; + float cornerY = centerY + 0.5 * cube_len * jj; + float cornerZ = centerZ + 0.5 * cube_len * kk; + + squaredSide1 = (state.pos_end(0) - cornerX) + * (state.pos_end(0) - cornerX) + + (state.pos_end(1) - cornerY) + * (state.pos_end(1) - cornerY) + + (state.pos_end(2) - cornerZ) + * (state.pos_end(2) - cornerZ); + + squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) + + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY) + + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ); + + ang_cos = fabs(squaredSide1 <= 3) ? 1.0 : + (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1)); + + if(ang_cos > HALF_FOV_COS) isInLaserFOV = true; + } + } + } + + if(!isInLaserFOV) + { + float cornerX = centerX; + float cornerY = centerY; + float cornerZ = centerZ; + + squaredSide1 = (state.pos_end(0) - cornerX) + * (state.pos_end(0) - cornerX) + + (state.pos_end(1) - cornerY) + * (state.pos_end(1) - cornerY) + + (state.pos_end(2) - cornerZ) + * (state.pos_end(2) - cornerZ); + + if(squaredSide1 <= 0.4 * cube_len * cube_len) + { + isInLaserFOV = true; + } + + squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) + + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY) + + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ); + + ang_cos = fabs(squaredSide2 <= 0.5 * cube_len) ? 1.0 : + (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1)); + + if(ang_cos > HALF_FOV_COS) isInLaserFOV = true; + } + + // std::cout<<"cent point: "<clear(); + + for (int i = 0; i < laserCloudValidNum; i++) + { + *featsFromMap += *featsArray[laserCloudValidInd[i]]; + } +} + +void feat_points_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + mtx_buffer.lock(); + // std::cout<<"got feature"<header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + lidar_buffer.clear(); + } + + // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); + lidar_buffer.push_back(msg); + last_timestamp_lidar = msg->header.stamp.toSec(); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) +{ + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); + + double timestamp = msg->header.stamp.toSec(); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + ROS_ERROR("imu loop back, clear buffer"); + imu_buffer.clear(); + b_reset = true; + b_first = true; + } + + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + // std::cout<<"got imu: "<header.stamp.toSec(); + lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); + lidar_pushed = true; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + + /*** push imu data, and pop from buffer ***/ + double imu_time = imu_buffer.front()->header.stamp.toSec(); + meas.imu.clear(); + while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_buffer.front()->header.stamp.toSec(); + meas.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); + } + + lidar_buffer.pop_front(); + lidar_pushed = false; + // std::cout<<"[IMU Sycned]: "< + // ("/livox_undistort", 100, featsLastHandler); + ros::Publisher pubLaserCloudFullRes = nh.advertise + ("/cloud_registered", 100); + ros::Publisher pubLaserCloudMap = nh.advertise + ("/Laser_map", 100); + // ros::Publisher pubSolvedPose6D = nh.advertise + // ("/States_updated", 100); + ros::Publisher pubOdomAftMapped = nh.advertise + ("/aft_mapped_to_init", 10); + +#ifdef DEPLOY + ros::Publisher mavros_pose_publisher = nh.advertise("/mavros/vision_pose/pose", 10); + geometry_msgs::PoseStamped msg_body_pose; +#endif + + /*** variables definition ***/ + bool dense_map_en, Need_Init = true; + std::string map_file_path; + int effect_feat_num = 0, frame_num = 0; + double filter_size_corner_min, filter_size_surf_min, filter_size_map_min, fov_deg,\ + deltaT, deltaR, aver_time_consu = 0, first_lidar_time = 0; + Eigen::Matrix G, H_T_H, I_STATE; + G.setZero(); + H_T_H.setZero(); + I_STATE.setIdentity(); + + nav_msgs::Odometry odomAftMapped; + + cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); + cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); + cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); + cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); + + PointType pointOri, pointSel, coeff; + PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); + PointCloudXYZI::Ptr feats_down(new PointCloudXYZI()); + PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI()); + PointCloudXYZI::Ptr coeffSel(new PointCloudXYZI()); + pcl::VoxelGrid downSizeFilterSurf; + pcl::VoxelGrid downSizeFilterMap; + + /*** variables initialize ***/ + ros::param::get("~dense_map_enable",dense_map_en); + ros::param::get("~max_iteration",NUM_MAX_ITERATIONS); + ros::param::get("~map_file_path",map_file_path); + ros::param::get("~fov_degree",fov_deg); + ros::param::get("~filter_size_corner",filter_size_corner_min); + ros::param::get("~filter_size_surf",filter_size_surf_min); + ros::param::get("~filter_size_map",filter_size_map_min); + ros::param::get("~cube_side_length",cube_len); + + HALF_FOV_COS = std::cos((fov_deg + 10.0) * 0.5 * PI_M / 180.0); + + for (int i = 0; i < laserCloudNum; i++) + { + featsArray[i].reset(new PointCloudXYZI()); + } + + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + + std::shared_ptr p_imu(new ImuProcess()); + + /*** debug record ***/ + std::ofstream fout_pre, fout_out; + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),std::ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),std::ios::out); + if (fout_pre && fout_out) + std::cout << "~~~~"< T1, s_plot, s_plot2, s_plot3; + +//------------------------------------------------------------------------------------------------------ + signal(SIGINT, SigHandle); + ros::Rate rate(5000); + bool status = ros::ok(); + while (status) + { + if (b_exit) break; + ros::spinOnce(); + while(sync_packages(Measures)) + { + if (b_reset) + { + ROS_WARN("reset when rosbag play back"); + p_imu->Reset(); + b_reset = false; + continue; + } + + double t1,t2,t3,t4,match_start, match_time, solve_start, solve_time, pca_time, svd_time; + match_time = 0; + solve_time = 0; + pca_time = 0; + svd_time = 0; + t1 = omp_get_wtime(); + + p_imu->Process(Measures, state, feats_undistort); + + if (feats_undistort->empty() || (feats_undistort == NULL)) + { + first_lidar_time = Measures.lidar_beg_time; + std::cout<<"not ready for odometry"<points.size(); + int feats_down_size = feats_down->points.size(); + std::cout<<"[ mapping ]: Raw feature num: "<points.size()<<" downsamp num "< 100) + { + kdtreeSurfFromMap->setInputCloud(featsFromMap); + std::vector point_selected_surf(feats_down_size, true); + std::vector> pointSearchInd_surf(feats_down_size); + + int rematch_num = 0; + bool rematch_en = 0; + + t2 = omp_get_wtime(); + + for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) + { + match_start = omp_get_wtime(); + laserCloudOri->clear(); + coeffSel->clear(); + + /** closest surface search and residual computation **/ + omp_set_num_threads(4); + #pragma omp parallel for + for (int i = 0; i < feats_down_size; i++) + { + PointType &pointOri_tmpt = feats_down->points[i]; + PointType &pointSel_tmpt = feats_down_updated->points[i]; + + /* transform to world frame */ + pointBodyToWorld(&pointOri_tmpt, &pointSel_tmpt); + + std::vector pointSearchSqDis_surf; + auto &points_near = pointSearchInd_surf[i]; + if (iterCount == 0 || rematch_en) + { + /** Find the closest surface/line in the map **/ + kdtreeSurfFromMap->nearestKSearch(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf); + if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3.0) + { + point_selected_surf[i] = true; + } + } + + if (! point_selected_surf[i]) continue; + + double pca_start = omp_get_wtime(); + + /// PCA (using minimum square method) + cv::Mat matA0(NUM_MATCH_POINTS, 3, CV_32F, cv::Scalar::all(0)); + cv::Mat matB0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(-1)); + cv::Mat matX0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(0)); + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + matA0.at(j, 0) = featsFromMap->points[points_near[j]].x; + matA0.at(j, 1) = featsFromMap->points[points_near[j]].y; + matA0.at(j, 2) = featsFromMap->points[points_near[j]].z; + } + + //matA0*matX0=matB0 + //AX+BY+CZ+D = 0 <=> AX+BY+CZ=-D <=> (A/D)X+(B/D)Y+(C/D)Z = -1 + //(X,Y,Z)<=>mat_a0 + //A/D, B/D, C/D <=> mat_x0 + + cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); //TODO + + float pa = matX0.at(0, 0); + float pb = matX0.at(1, 0); + float pc = matX0.at(2, 0); + float pd = 1; + + //ps is the norm of the plane norm_vec vector + //pd is the distance from point to plane + float ps = sqrt(pa * pa + pb * pb + pc * pc); + pa /= ps; + pb /= ps; + pc /= ps; + pd /= ps; + + bool planeValid = true; + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + if (fabs(pa * featsFromMap->points[points_near[j]].x + + pb * featsFromMap->points[points_near[j]].y + + pc * featsFromMap->points[points_near[j]].z + pd) > 0.1) + { + planeValid = false; + point_selected_surf[i] = false; + break; + } + } + + if (planeValid) + { + //loss fuction + float pd2 = pa * pointSel_tmpt.x + pb * pointSel_tmpt.y + pc * pointSel_tmpt.z + pd; + //if(fabs(pd2) > 0.1) continue; + float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y + pointSel_tmpt.z * pointSel_tmpt.z)); + + if (s > 0.1) + { + point_selected_surf[i] = true; + coeffSel_tmpt->points[i].x = s * pa; + coeffSel_tmpt->points[i].y = s * pb; + coeffSel_tmpt->points[i].z = s * pc; + coeffSel_tmpt->points[i].intensity = s * pd2; + } + else + { + point_selected_surf[i] = false; + } + } + + pca_time += omp_get_wtime() - pca_start; + } + + double total_residual = 0.0; + for (int i = 0; i < coeffSel_tmpt->points.size(); i++) + { + float error_abs = std::abs(coeffSel_tmpt->points[i].intensity); + if (point_selected_surf[i] && (error_abs < 0.5)) + { + laserCloudOri->push_back(feats_down->points[i]); + coeffSel->push_back(coeffSel_tmpt->points[i]); + total_residual += error_abs; + } + } + + int laserCloudSelNum = laserCloudOri->points.size(); + double ave_residual = total_residual / laserCloudSelNum; + // ave_res_last + + effect_feat_num = coeffSel->size(); + if(iterCount == 1) std::cout << "[ mapping ]: Effective feature num: "<points[i]; + Eigen::Vector3d point_this(laser_p.x, laser_p.y, laser_p.z); + point_this += Lidar_offset_to_IMU; + Eigen::Matrix3d point_crossmat; + point_crossmat<points[i]; + Eigen::Vector3d norm_vec(norm_p.x, norm_p.y, norm_p.z); + + /*** calculate the Measuremnt Jacobian matrix H ***/ + Eigen::Vector3d A(point_crossmat * state.rot_end.transpose() * norm_vec); + Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z; + + /*** Measuremnt: distance to the closest surface/corner ***/ + meas_vec(i) = - norm_p.intensity; + } + + Eigen::Vector3d rot_add, t_add, v_add, bg_add, ba_add, g_add; + Eigen::VectorXd solution(DIM_OF_STATES); + Eigen::MatrixXd K(DIM_OF_STATES, laserCloudSelNum); + + /*** Iterative Kalman Filter Update ***/ + if (Need_Init) + { + /*** only run in initialization period ***/ + Eigen::MatrixXd H_init(Eigen::Matrix::Zero()); + Eigen::MatrixXd z_init(Eigen::Matrix::Zero()); + H_init.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); + H_init.block<3,3>(3,3) = Eigen::Matrix3d::Identity(); + H_init.block<3,3>(6,15) = Eigen::Matrix3d::Identity(); + z_init.block<3,1>(0,0) = - Log(state.rot_end); + z_init.block<3,1>(0,0) = - state.pos_end; + + auto H_init_T = H_init.transpose(); + auto &&K_init = state.cov * H_init_T * (H_init * state.cov * H_init_T + 0.0001 * Eigen::Matrix::Identity()).inverse(); + solution = K_init * z_init; + + solution.block<9,1>(0,0).setZero(); + state += solution; + state.cov = (Eigen::MatrixXd::Identity(DIM_OF_STATES, DIM_OF_STATES) - K_init * H_init) * state.cov; + } + else + { + auto &&Hsub_T = Hsub.transpose(); + H_T_H.block<6,6>(0,0) = Hsub_T * Hsub; + Eigen::Matrix &&K_1 = (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse(); + K = K_1.block(0,0) * Hsub_T; + solution = K * meas_vec; + state += solution; + + rot_add = solution.block<3,1>(0,0); + t_add = solution.block<3,1>(3,0); + + deltaR = rot_add.norm() * 57.3; + deltaT = t_add.norm() * 100.0; + } + + euler_cur = RotMtoEuler(state.rot_end); + + #ifdef DEBUG_PRINT + // std::cout<<"solution: "<= 2 || (iterCount == NUM_MAX_ITERATIONS - 1)) + { + if (!Need_Init) + { + /*** Covariance Update ***/ + G.block(0,0) = K * Hsub; + state.cov = (I_STATE - G) * state.cov; + // std::cout<<"propagated cov: "<points[i]; + + int cubeI = int((pointSel.x + 0.5 * cube_len) / cube_len) + laserCloudCenWidth; + int cubeJ = int((pointSel.y + 0.5 * cube_len) / cube_len) + laserCloudCenHeight; + int cubeK = int((pointSel.z + 0.5 * cube_len) / cube_len) + laserCloudCenDepth; + + if (pointSel.x + 0.5 * cube_len < 0) cubeI--; + if (pointSel.y + 0.5 * cube_len < 0) cubeJ--; + if (pointSel.z + 0.5 * cube_len < 0) cubeK--; + + if (cubeI >= 0 && cubeI < laserCloudWidth && + cubeJ >= 0 && cubeJ < laserCloudHeight && + cubeK >= 0 && cubeK < laserCloudDepth) { + int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; + featsArray[cubeInd]->push_back(pointSel); + if_cube_updated[cubeInd] = true; + } + } + for (int i = 0; i < laserCloudValidNum; i++) + { + int ind = laserCloudValidInd[i]; + + if(if_cube_updated[ind]) + { + downSizeFilterMap.setInputCloud(featsArray[ind]); + downSizeFilterMap.filter(*featsArray[ind]); + } + } + t3 = omp_get_wtime(); + + /******* Publish current frame points in world coordinates: *******/ + laserCloudFullRes2->clear(); + *laserCloudFullRes2 = dense_map_en ? (*feats_undistort) : (* feats_down); + // *laserCloudFullRes2 = dense_map_en ? (*laserCloudFullRes) : (* feats_down); + + int laserCloudFullResNum = laserCloudFullRes2->points.size(); + + pcl::PointXYZRGB temp_point; + laserCloudFullResColor->clear(); + for (int i = 0; i < laserCloudFullResNum; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes2->points[i], &temp_point); + laserCloudFullResColor->push_back(temp_point); + } + + sensor_msgs::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar); + laserCloudFullRes3.header.frame_id = "/camera_init"; + pubLaserCloudFullRes.publish(laserCloudFullRes3); + + /******* Publish Maps: *******/ + // sensor_msgs::PointCloud2 laserCloudMap; + // pcl::toROSMsg(*featsFromMap, laserCloudMap); + // laserCloudMap.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); + // laserCloudMap.header.frame_id = "/camera_init"; + // pubLaserCloudMap.publish(laserCloudMap); + + /******* Publish Odometry ******/ + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw + (euler_cur(2), - euler_cur(0), - euler_cur(1)); + odomAftMapped.header.frame_id = "/camera_init"; + odomAftMapped.child_frame_id = "/aft_mapped"; + odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); + odomAftMapped.pose.pose.orientation.x = -geoQuat.y; + odomAftMapped.pose.pose.orientation.y = -geoQuat.z; + odomAftMapped.pose.pose.orientation.z = geoQuat.x; + odomAftMapped.pose.pose.orientation.w = geoQuat.w; + odomAftMapped.pose.pose.position.x = state.pos_end(0); + odomAftMapped.pose.pose.position.y = state.pos_end(1); + odomAftMapped.pose.pose.position.z = state.pos_end(2); + + pubOdomAftMapped.publish(odomAftMapped); + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin( tf::Vector3( odomAftMapped.pose.pose.position.x, + odomAftMapped.pose.pose.position.y, + odomAftMapped.pose.pose.position.z ) ); + q.setW( odomAftMapped.pose.pose.orientation.w ); + q.setX( odomAftMapped.pose.pose.orientation.x ); + q.setY( odomAftMapped.pose.pose.orientation.y ); + q.setZ( odomAftMapped.pose.pose.orientation.z ); + transform.setRotation( q ); + br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped" ) ); + + #ifdef DEPLOY + msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.frame_id = "/camera_odom_frame"; + msg_body_pose.pose.position.x = state.pos_end(0); + msg_body_pose.pose.position.y = - state.pos_end(1); + msg_body_pose.pose.position.z = - state.pos_end(2); + msg_body_pose.pose.orientation.x = - geoQuat.y; + msg_body_pose.pose.orientation.y = - geoQuat.z; + msg_body_pose.pose.orientation.z = geoQuat.x; + msg_body_pose.pose.orientation.w = geoQuat.w; + mavros_pose_publisher.publish(msg_body_pose); + #endif + + /*** save debug variables ***/ + t4 = omp_get_wtime(); + frame_num ++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t3 - t1) / frame_num; + // aver_time_consu = aver_time_consu * 0.5 + (t4 - t1) * 0.5; + T1.push_back(Measures.lidar_beg_time); + s_plot.push_back(aver_time_consu); + // s_plot2.push_back(double(deltaR)); + // s_plot3.push_back(double(deltaT)); + + std::cout<<"[ mapping ]: time: selection "< 0 && corner_points.size() > 0) + { + pcl::PCDWriter pcd_writer; + std::cout << "saving..."; + pcd_writer.writeBinary(surf_filename, surf_points); + pcd_writer.writeBinary(corner_filename, corner_points); + } + else + { + // #ifdef DEBUG_PRINT + #ifndef DEPLOY + if (!T1.empty()) + { + plt::named_plot("time consumed",T1,s_plot); + // plt::named_plot("R_residual",T1,s_plot2); + // plt::named_plot("T_residual",T1,s_plot3); + plt::legend(); + plt::show(); + plt::pause(0.5); + plt::close(); + // plt::save("/home/xw/catkin_like_loam/src/LIEK_LOAM/a.png"); + } + std::cout << "no points saved"; + // #endif + #endif + } + //-------------------------- + // loss_output.close(); + return 0; +} diff --git a/tools/plot.py b/tools/plot.py new file mode 100644 index 0000000..eb8ef7d --- /dev/null +++ b/tools/plot.py @@ -0,0 +1,86 @@ +# import matplotlib +# matplotlib.use('Agg') +import numpy as np +import matplotlib.pyplot as plt + +fig, axs = plt.subplots(5) +lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] +lab_out = ['', 'out-x', 'out-y', 'out-z'] +plot_ind = range(7,10) +a_pre=np.loadtxt('Log/mat_pre.txt') +a_out=np.loadtxt('Log/mat_out.txt') +time=a_pre[:,0] +axs[0].set_title('Attitude') +axs[1].set_title('Translation') +axs[2].set_title('Velocity') +axs[3].set_title('bg') +axs[4].set_title('ba') +for i in range(1,4): + for j in range(5): + axs[j].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) + axs[j].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) + +for i in range(5): + # axs[i].set_xlim(386,389) + axs[i].grid() + axs[i].legend() + + +# #### Draw IMU data +# fig, axs = plt.subplots(2) +# imu=np.loadtxt('Log/imu_0.txt') +# time=imu[:,0] +# axs[0].set_title('Gyroscope') +# axs[1].set_title('Accelerameter') +# lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] +# lab_2 = ['acc-x', 'acc-y', 'acc-z'] +# for i in range(3): +# # if i==1: +# axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) +# axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) +# for i in range(2): +# axs[i].set_xlim(386,389) +# axs[i].grid() +# axs[i].legend() + +# #### Draw time calculation +# fig = plt.figure() +# font1 = {'family' : 'Times New Roman', +# 'weight' : 'normal', +# 'size' : 12, +# } +# c="red" +# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') +# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') +# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') +# # n = a_out[:,1].size +# # time_mean = a_out[:,1].mean() +# # time_se = a_out[:,1].std() / np.sqrt(n) +# # time_err = a_out[:,1] - time_mean +# # feat_mean = a_out[:,2].mean() +# # feat_err = a_out[:,2] - feat_mean +# # feat_se = a_out[:,2].std() / np.sqrt(n) +# ax1 = fig.add_subplot(111) +# ax1.set_ylabel('Effective Feature Numbers',font1) +# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) +# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) +# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) +# ax1.set_ylim([0, 3000]) + +# ax2 = ax1.twinx() +# ax2.spines['right'].set_color('red') +# ax2.set_ylabel('Compute Time (ms)',font1) +# ax2.yaxis.label.set_color('red') +# ax2.tick_params(axis='y', colors='red') +# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.set_xlim([0.5, 3.5]) +# ax2.set_ylim([0, 100]) + +# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) +# # print(time_se) +# # print(a_out3[:,2]) +plt.grid() +plt.savefig("Log/time.pdf", dpi=1200) +plt.show() diff --git a/tools/plot_state.py b/tools/plot_state.py new file mode 100644 index 0000000..e69de29