├── README.md ├── lidar_imu_calib ├── CMakeLists.txt ├── include │ └── calibExRLidar2Imu.h ├── launch │ └── calib_exR_lidar2imu.launch ├── package.xml └── src │ ├── calib_exR_lidar2imu_node.cpp │ └── lib │ └── calibExRLidar2Imu.cpp └── ndt_omp ├── .clang-format ├── CMakeLists.txt ├── LICENSE ├── README.md ├── apps └── align.cpp ├── data ├── 251370668.pcd ├── 251371071.pcd └── screenshot.png ├── include └── pclomp │ ├── gicp_omp.h │ ├── gicp_omp_impl.hpp │ ├── ndt_omp.h │ ├── ndt_omp_impl.hpp │ ├── voxel_grid_covariance_omp.h │ └── voxel_grid_covariance_omp_impl.hpp ├── package.xml └── src └── pclomp ├── gicp_omp.cpp ├── ndt_omp.cpp └── voxel_grid_covariance_omp.cpp /README.md: -------------------------------------------------------------------------------- 1 | # lidar_imu_calib 2 | 3 | ### overview 4 | 5 | when develop slam based on 3D lidar, we often use imu to provide priori for matching algorithm(icp, ndt), so the transform between lidar and imu need to be calibrated.For matching algorithm, attitude in transfom is more important than position in transform, and position often be set to 0. So this repo concentrate on calibrate attitude component in transform between lidar and imu. 6 | 7 | ### prerequisite 8 | 9 | - [ROS](http://wiki.ros.org/kinetic/Installation/Ubuntu) 10 | ### compile 11 | ``` 12 | mkdir -p catkin_ws/src 13 | cd catkin_ws/src 14 | git clone https://github.com/chennuo0125-HIT/lidar_imu_calib.git 15 | cd .. 16 | catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp;lidar_imu_calib" 17 | ``` 18 | ### run step 19 | 20 | 1. use rosbag tool record imu and lidar data 21 | 22 | ``` 23 | rosbag record /imu /lidar_points 24 | ``` 25 | 26 | 2. config launch file 27 | 28 | ``` 29 | lidar_topic: lidar data topic name 30 | imu_topic: imu data topic name 31 | bag_file: *.bag file record imu and lidar data topic 32 | ``` 33 | 34 | 35 | 3. start 36 | 37 | ``` 38 | roslaunch lidar_imu_calib calib_exR_lidar2imu.launch 39 | ``` 40 | ### reference 41 | [https://blog.csdn.net/weixin_37835423/article/details/110672571](https://blog.csdn.net/weixin_37835423/article/details/110672571) 42 | 43 | -------------------------------------------------------------------------------- /lidar_imu_calib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_imu_calib) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-o3) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | tf 16 | rosbag 17 | ) 18 | 19 | find_package(cmake_modules REQUIRED) 20 | find_package(Eigen3 REQUIRED) 21 | 22 | find_package(PCL 1.7 REQUIRED) 23 | 24 | catkin_package( 25 | # INCLUDE_DIRS include 26 | # LIBRARIES lidar_imu_calib 27 | # CATKIN_DEPENDS roscpp rospy sensor_msgs 28 | # DEPENDS system_lib 29 | ) 30 | 31 | ## Specify additional locations of header files 32 | ## Your package locations should be listed before other locations 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | 38 | include_directories(${EIGEN3_INCLUDE_DIR}) 39 | add_definitions(${EIGEN3_DEFINITIONS}) 40 | 41 | include_directories(${PCL_INCLUDE_DIRS}) 42 | link_directories(${PCL_LIBRARY_DIRS}) 43 | add_definitions(${PCL_DEFINITIONS}) 44 | 45 | add_library(calib src/lib/calibExRLidar2Imu.cpp) 46 | target_link_libraries(calib ${catkin_LIBRARIES} ${PCL_LIBRARIES} ndt_omp) 47 | add_executable(calib_exR_lidar2imu_node src/calib_exR_lidar2imu_node.cpp) 48 | target_link_libraries(calib_exR_lidar2imu_node calib) 49 | -------------------------------------------------------------------------------- /lidar_imu_calib/include/calibExRLidar2Imu.h: -------------------------------------------------------------------------------- 1 | /* 2 | * brief: calibrate extrincs rotation between multi-layer lidar and imu 3 | * author: chennuo0125@163.com 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "../../ndt_omp/include/pclomp/ndt_omp.h" 17 | 18 | using namespace std; 19 | using PointT = pcl::PointXYZI; 20 | using CloudT = pcl::PointCloud; 21 | 22 | struct LidarData 23 | { 24 | double stamp; 25 | CloudT::Ptr cloud; 26 | }; 27 | 28 | struct LidarFrame 29 | { 30 | double stamp; 31 | Eigen::Matrix4d T; 32 | Eigen::Matrix4d gT; 33 | CloudT::Ptr cloud{nullptr}; 34 | 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | }; 37 | 38 | struct ImuData 39 | { 40 | double stamp; 41 | Eigen::Vector3d acc; 42 | Eigen::Vector3d gyr; 43 | Eigen::Quaterniond rot; 44 | 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | }; 47 | 48 | class CalibExRLidarImu 49 | { 50 | public: 51 | CalibExRLidarImu(); 52 | ~CalibExRLidarImu(); 53 | 54 | //@brief: set init extrinsic if have 55 | void setInitExR(Eigen::Vector3d init_R); 56 | 57 | //@brief: add lidar data and calculate lidar odometry 58 | void addLidarData(const LidarData &data); 59 | 60 | //@brief: add imu data and cache 61 | void addImuData(const ImuData &data); 62 | 63 | //@brief: integration imu data, align lidar odom and imu 64 | Eigen::Vector3d calib(bool integration = false); 65 | 66 | private: 67 | //@brief: interpolated attitude from start attitude to end attitude by scale 68 | Eigen::Quaterniond getInterpolatedAttitude(const Eigen::Quaterniond &q_s_w, const Eigen::Quaterniond &q_e_w, double scale); 69 | 70 | //@brief: update relative transform between neighbor lidar frame by aligned imu data 71 | void optimize(); 72 | 73 | //@brief: solve least square answer by constraints 74 | Eigen::Quaterniond solve(const vector> &corres); 75 | 76 | Eigen::Vector3d init_R_{0.0, 0.0, 0.0}; 77 | CloudT::Ptr last_lidar_cloud_{nullptr}; // last lidar cloud 78 | vector lidar_buffer_; // record relative transform between neighbor lidar frame 79 | vector imu_buffer_; // record raw imu datas 80 | vector> aligned_lidar_imu_buffer_; // aligned lidar frame and interpolated imu attitude at lidar stamp 81 | Eigen::Quaterniond q_l_b_; // result 82 | 83 | CloudT::Ptr local_map_{nullptr}; // local map 84 | pcl::VoxelGrid downer_; // downsample local map 85 | pclomp::NormalDistributionsTransform::Ptr register_{nullptr}; // register object 86 | 87 | vector> corres1_; 88 | vector> corres2_; 89 | 90 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 91 | }; -------------------------------------------------------------------------------- /lidar_imu_calib/launch/calib_exR_lidar2imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /lidar_imu_calib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_imu_calib 4 | 0.0.0 5 | The lidar_imu_calib package 6 | 7 | 8 | 9 | 10 | cn 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | sensor_msgs 55 | roscpp 56 | rospy 57 | sensor_msgs 58 | roscpp 59 | rospy 60 | sensor_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /lidar_imu_calib/src/calib_exR_lidar2imu_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "calibExRLidar2Imu.h" 11 | 12 | #include 13 | #define foreach BOOST_FOREACH 14 | 15 | #define READ_BAGFILE true 16 | 17 | using namespace std; 18 | 19 | queue lidar_buffer; 20 | queue imu_buffer; 21 | 22 | void lidarCallback(const sensor_msgs::PointCloud2ConstPtr &msg) 23 | { 24 | lidar_buffer.push(msg); 25 | } 26 | 27 | void imuCallback(const sensor_msgs::ImuConstPtr &msg) 28 | { 29 | imu_buffer.push(msg); 30 | } 31 | 32 | int main(int argc, char **argv) 33 | { 34 | ros::init(argc, argv, "calib_exR_lidar2imu_node"); 35 | ros::NodeHandle nh, pnh("~"); 36 | 37 | // read data topic 38 | string lidar_topic, imu_topic; 39 | if (!pnh.getParam("lidar_topic", lidar_topic) || !pnh.getParam("imu_topic", imu_topic)) 40 | { 41 | cout << "please config param: lidar_topic, imu_topic !!!" << endl; 42 | return 0; 43 | } 44 | 45 | // initialize caliber 46 | CalibExRLidarImu caliber; 47 | 48 | #if READ_BAGFILE 49 | // get local param 50 | string bag_file; 51 | if (!pnh.getParam("bag_file", bag_file)) 52 | { 53 | cout << "please config param: bag_file !!!" << endl; 54 | return 0; 55 | } 56 | 57 | // open bagfile 58 | rosbag::Bag bag; 59 | bag.open(bag_file, rosbag::bagmode::Read); 60 | vector topics; 61 | topics.push_back(lidar_topic); 62 | topics.push_back(imu_topic); 63 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 64 | 65 | // read data and add data 66 | foreach (rosbag::MessageInstance const m, view) 67 | { 68 | // add lidar msg 69 | sensor_msgs::PointCloud2ConstPtr lidar_msg = m.instantiate(); 70 | if (lidar_msg != NULL) 71 | { 72 | ROS_INFO_STREAM_THROTTLE(5.0, "add lidar msg ......"); 73 | 74 | CloudT::Ptr cloud(new CloudT); 75 | pcl::fromROSMsg(*lidar_msg, *cloud); 76 | LidarData data; 77 | data.cloud = cloud; 78 | data.stamp = lidar_msg->header.stamp.toSec(); 79 | caliber.addLidarData(data); 80 | } 81 | 82 | // add imu msg 83 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 84 | if (imu_msg) 85 | { 86 | ImuData data; 87 | data.acc = Eigen::Vector3d(imu_msg->linear_acceleration.x, 88 | imu_msg->linear_acceleration.y, 89 | imu_msg->linear_acceleration.z); 90 | data.gyr = Eigen::Vector3d(imu_msg->angular_velocity.x, 91 | imu_msg->angular_velocity.y, 92 | imu_msg->angular_velocity.z); 93 | data.rot = Eigen::Quaterniond(imu_msg->orientation.w, 94 | imu_msg->orientation.x, 95 | imu_msg->orientation.y, 96 | imu_msg->orientation.z); 97 | data.stamp = imu_msg->header.stamp.toSec(); 98 | caliber.addImuData(data); 99 | } 100 | } 101 | #else 102 | ros::Subscriber lidar_sub = nh.subscribe("/cloud", 1000, lidarCallback); 103 | ros::Subscriber imu_sub = nh.subscribe("/imu", 10000, imuCallback); 104 | 105 | // add data 106 | ros::Rate loop_rate(200); 107 | while (ros::ok()) 108 | { 109 | ros::spinOnce(); 110 | 111 | ROS_INFO_STREAM_THROTTLE(5.0, "lidar buffer size " << lidar_buffer.size() << ", imu buffer size " << imu_buffer.size()); // monitor 112 | 113 | // add lidar data 114 | while (lidar_buffer.size() != 0) 115 | { 116 | CloudT::Ptr cloud(new CloudT); 117 | pcl::fromROSMsg(*(lidar_buffer.front()), *cloud); 118 | LidarData data; 119 | data.cloud = cloud; 120 | data.stamp = lidar_buffer.front()->header.stamp.toSec(); 121 | caliber.addLidarData(data); 122 | lidar_buffer.pop(); 123 | } 124 | 125 | // add imu data 126 | while (imu_buffer.size() != 0) 127 | { 128 | ImuData data; 129 | data.acc = Eigen::Vector3d(imu_buffer.front()->linear_acceleration.x, 130 | imu_buffer.front()->linear_acceleration.y, 131 | imu_buffer.front()->linear_acceleration.z); 132 | data.gyr = Eigen::Vector3d(imu_buffer.front()->angular_velocity.x, 133 | imu_buffer.front()->angular_velocity.y, 134 | imu_buffer.front()->angular_velocity.z); 135 | data.rot = Eigen::Quaterniond(imu_buffer.front()->orientation.w, 136 | imu_buffer.front()->orientation.x, 137 | imu_buffer.front()->orientation.y, 138 | imu_buffer.front()->orientation.z); 139 | data.stamp = imu_buffer.front()->header.stamp.toSec(); 140 | caliber.addImuData(data); 141 | imu_buffer.pop(); 142 | } 143 | 144 | loop_rate.sleep(); 145 | } 146 | #endif 147 | 148 | // calib 149 | Eigen::Vector3d rpy = caliber.calib(); 150 | Eigen::Matrix3d rot = Eigen::Matrix3d(Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX())); 151 | cout << "result euler angle(RPY) : " << rpy[0] << " " << rpy[1] << " " << rpy[2] << endl; 152 | cout << "result extrinsic rotation matrix : " << endl; 153 | cout << rot << endl; 154 | 155 | return 0; 156 | } -------------------------------------------------------------------------------- /lidar_imu_calib/src/lib/calibExRLidar2Imu.cpp: -------------------------------------------------------------------------------- 1 | #include "calibExRLidar2Imu.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define USE_SCAN_2_MAP true 8 | 9 | CalibExRLidarImu::CalibExRLidarImu() 10 | { 11 | imu_buffer_.clear(); 12 | 13 | // init downsample object 14 | downer_.setLeafSize(0.1, 0.1, 0.1); 15 | 16 | // init register object 17 | register_.reset(new pclomp::NormalDistributionsTransform()); 18 | register_->setResolution(1.0); 19 | int avalib_cpus = omp_get_max_threads(); 20 | register_->setNumThreads(avalib_cpus); 21 | register_->setNeighborhoodSearchMethod(pclomp::DIRECT7); 22 | } 23 | 24 | CalibExRLidarImu::~CalibExRLidarImu() 25 | { 26 | } 27 | 28 | void CalibExRLidarImu::setInitExR(Eigen::Vector3d init_R) 29 | { 30 | init_R_ = init_R; 31 | } 32 | 33 | void CalibExRLidarImu::addLidarData(const LidarData &data) 34 | { 35 | if (!data.cloud || data.cloud->size() == 0) 36 | { 37 | cout << "no cloud in lidar data !!!" << endl; 38 | return; 39 | } 40 | 41 | if (!register_) 42 | { 43 | cout << "register no initialize !!!" << endl; 44 | return; 45 | } 46 | 47 | #if USE_SCAN_2_MAP 48 | //downsample lidar cloud for save align time 49 | CloudT::Ptr downed_cloud(new CloudT); 50 | downer_.setInputCloud(data.cloud); 51 | downer_.filter(*downed_cloud); 52 | 53 | if (!local_map_) 54 | { 55 | local_map_.reset(new CloudT); 56 | *local_map_ += *(data.cloud); 57 | 58 | LidarFrame frame; 59 | frame.stamp = data.stamp; 60 | frame.T = Eigen::Matrix4d::Identity(); 61 | frame.gT = Eigen::Matrix4d::Identity(); 62 | frame.cloud = downed_cloud; 63 | lidar_buffer_.push_back(move(frame)); 64 | 65 | return; 66 | } 67 | 68 | // downsample local map for save align time 69 | CloudT::Ptr downed_map(new CloudT); 70 | downer_.setInputCloud(local_map_); 71 | downer_.filter(*downed_map); 72 | local_map_ = downed_map; 73 | 74 | // get transform between frame and local map 75 | register_->setInputSource(downed_cloud); 76 | register_->setInputTarget(local_map_); 77 | CloudT::Ptr aligned(new CloudT); 78 | register_->align(*aligned); 79 | if (!register_->hasConverged()) 80 | { 81 | cout << "register cant converge, please check initial value !!!" << endl; 82 | return; 83 | } 84 | Eigen::Matrix4d T_l_m = (register_->getFinalTransformation()).cast(); 85 | 86 | // generate lidar frame 87 | LidarFrame frame; 88 | frame.stamp = data.stamp; 89 | frame.gT = T_l_m; 90 | Eigen::Matrix4d last_T_l_m = lidar_buffer_.back().gT; 91 | frame.T = last_T_l_m.inverse() * T_l_m; 92 | frame.cloud = downed_cloud; 93 | lidar_buffer_.push_back(move(frame)); 94 | 95 | // update local map 96 | *local_map_ += *aligned; 97 | #else 98 | // init first lidar frame and set it as zero pose 99 | if (!last_lidar_cloud_) 100 | { 101 | last_lidar_cloud_.reset(new CloudT); 102 | last_lidar_cloud_ = data.cloud; 103 | 104 | LidarFrame frame; 105 | frame.stamp = data.stamp; 106 | frame.T = Eigen::Matrix4d::Identity(); 107 | frame.gT = Eigen::Matrix4d::Identity(); 108 | lidar_buffer_.push_back(move(frame)); 109 | 110 | return; 111 | } 112 | 113 | // get transform between neighbor frames 114 | register_->setInputSource(data.cloud_); 115 | register_->setInputTarget(last_lidar_cloud); 116 | CloudT::Ptr aligned(new CloudT); 117 | register_->align(*aligned); 118 | 119 | if (!register_->hasConverged()) 120 | { 121 | cout << "register cant converge, please check initial value !!!" << endl; 122 | return; 123 | } 124 | Eigen::Matrix4d result_T = (register_->getFinalTransformation()).cast(); 125 | 126 | // generate lidar frame 127 | LidarFrame frame; 128 | frame.stamp = data.stamp; 129 | frame.T = result_T; 130 | Eigen::Matrix4d temp1 = lidar_buffer_.back().gT; 131 | Eigen::Matrix4d temp2 = result_T; 132 | frame.gT = temp1 * temp2; 133 | lidar_buffer_.push_back(move(frame)); 134 | 135 | // debug 136 | // CloudT::Ptr g_cloud(new CloudT); 137 | // pcl::transformPointCloud(*(data.cloud), *g_cloud, lidar_buffer_.back().gT.cast()); 138 | // string pcd_file = "/home/cn/temp/" + to_string(lidar_buffer_.size()) + ".pcd"; 139 | // pcl::io::savePCDFile(pcd_file, *g_cloud); 140 | #endif 141 | } 142 | 143 | void CalibExRLidarImu::addImuData(const ImuData &data) 144 | { 145 | imu_buffer_.push_back(data); 146 | } 147 | 148 | Eigen::Quaterniond CalibExRLidarImu::getInterpolatedAttitude(const Eigen::Quaterniond &q_s_w, const Eigen::Quaterniond &q_e_w, double scale) 149 | { 150 | if (0 == scale || scale > 1) 151 | return move(Eigen::Quaterniond().Identity()); 152 | 153 | // calculate angleaxis difference 154 | Eigen::Quaterniond q_e_s = q_s_w.inverse() * q_e_w; 155 | q_e_s.normalize(); 156 | Eigen::AngleAxisd diff_angle_axis(q_e_s); 157 | 158 | // interpolated attitude by scale 159 | double interpolated_angle = diff_angle_axis.angle() * scale; 160 | Eigen::Quaterniond q_ie_s(Eigen::AngleAxisd(interpolated_angle, diff_angle_axis.axis()).toRotationMatrix()); 161 | Eigen::Quaterniond q_ie_w = q_s_w * q_ie_s; 162 | q_ie_w.normalize(); 163 | 164 | return move(q_ie_w); 165 | } 166 | 167 | Eigen::Quaterniond CalibExRLidarImu::solve(const vector> &corres) 168 | { 169 | if (corres.size() == 0) 170 | { 171 | cout << "no constraint found !!!" << endl; 172 | return move(Eigen::Quaterniond().Identity()); 173 | } 174 | 175 | cout << "constraints size " << corres.size() << endl; 176 | 177 | // transform quaternion to skew symmetric matrix 178 | auto toSkewSymmetric = [](const Eigen::Vector3d &q) -> Eigen::Matrix3d { 179 | Eigen::Matrix3d mat = Eigen::Matrix3d::Zero(); 180 | mat(0, 1) = -q.z(); 181 | mat(0, 2) = q.y(); 182 | mat(1, 0) = q.z(); 183 | mat(1, 2) = -q.x(); 184 | mat(2, 0) = -q.y(); 185 | mat(2, 1) = q.x(); 186 | 187 | return move(mat); 188 | }; 189 | 190 | // create homogeneous linear equations 191 | Eigen::MatrixXd A(corres.size() * 4, 4); 192 | for (int i = 0; i < corres.size(); i++) 193 | { 194 | // get relative transform 195 | const auto &q_l2_l1 = corres[i].first; 196 | const auto &q_b2_b1 = corres[i].second; 197 | 198 | // get left product matrix 199 | Eigen::Vector3d q_b2_b1_vec = q_b2_b1.vec(); 200 | Eigen::Matrix4d left_Q_b2_b1 = Eigen::Matrix4d::Zero(); 201 | left_Q_b2_b1.block<1, 3>(0, 1) = -q_b2_b1_vec.transpose(); 202 | left_Q_b2_b1.block<3, 1>(1, 0) = q_b2_b1_vec; 203 | left_Q_b2_b1.block<3, 3>(1, 1) = toSkewSymmetric(q_b2_b1_vec); 204 | left_Q_b2_b1 += q_b2_b1.w() * Eigen::Matrix4d::Identity(); 205 | 206 | // get right product matrix 207 | Eigen::Vector3d q_l2_l1_vec = q_l2_l1.vec(); 208 | Eigen::Matrix4d right_Q_l2_l1 = Eigen::Matrix4d::Zero(); 209 | right_Q_l2_l1.block<1, 3>(0, 1) = -q_l2_l1_vec.transpose(); 210 | right_Q_l2_l1.block<3, 1>(1, 0) = q_l2_l1_vec; 211 | right_Q_l2_l1.block<3, 3>(1, 1) = -toSkewSymmetric(q_l2_l1_vec); 212 | right_Q_l2_l1 += q_l2_l1.w() * Eigen::Matrix4d::Identity(); 213 | 214 | // add loss function 215 | double angle_distance = 180.0 / M_PI * q_b2_b1.angularDistance(q_l2_l1); 216 | double huber = angle_distance > 2.0 ? 2.0 / angle_distance : 1.0; 217 | 218 | A.block<4, 4>(i * 4, 0) = huber * (left_Q_b2_b1 - right_Q_l2_l1); 219 | } 220 | 221 | // solve homogeneous linear equations by svd method 222 | Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); 223 | Eigen::Matrix x = svd.matrixV().col(3); 224 | Eigen::Quaterniond q_l_b(x(0), x(1), x(2), x(3)); 225 | 226 | return move(q_l_b); 227 | } 228 | 229 | void CalibExRLidarImu::optimize() 230 | { 231 | if (aligned_lidar_imu_buffer_.size() == 0 || !register_) 232 | { 233 | cout << "no aligned data or register !!!" << endl; 234 | return; 235 | } 236 | 237 | // clear local map and initialize 238 | if (local_map_) 239 | local_map_->clear(); 240 | else 241 | local_map_.reset(new CloudT); 242 | *local_map_ += *(aligned_lidar_imu_buffer_[0].first.cloud); 243 | 244 | // use scan2match with estimated initial value to update lidar frame 245 | for (int i = 1; i < aligned_lidar_imu_buffer_.size(); i++) 246 | { 247 | // get front and back frames 248 | auto &aligned1 = aligned_lidar_imu_buffer_[i - 1]; 249 | auto &aligned2 = aligned_lidar_imu_buffer_[i]; 250 | 251 | // downsample local map and lidar cloud for save align time 252 | CloudT::Ptr downed_map(new CloudT); 253 | downer_.setInputCloud(local_map_); 254 | downer_.filter(*downed_map); 255 | local_map_ = downed_map; 256 | 257 | // calculate estimated T_l_m 258 | Eigen::Matrix3d R_l1_m = aligned1.first.gT.block<3, 3>(0, 0); 259 | Eigen::Quaterniond q_b1_w = aligned1.second; 260 | Eigen::Quaterniond q_b2_w = aligned2.second; 261 | Eigen::Quaterniond est_q_b2_b1 = q_b1_w.inverse() * q_b2_w; 262 | Eigen::Matrix3d est_R_l2_l1 = Eigen::Matrix3d(q_l_b_.inverse() * est_q_b2_b1 * q_l_b_); 263 | Eigen::Matrix3d est_R_l2_m = R_l1_m * est_R_l2_l1; 264 | Eigen::Matrix4d est_T_l2_m = Eigen::Matrix4d::Identity(); 265 | est_T_l2_m.block<3, 3>(0, 0) = est_R_l2_m; 266 | 267 | // register 268 | register_->setInputSource(aligned2.first.cloud); 269 | register_->setInputTarget(local_map_); 270 | CloudT::Ptr aligned(new CloudT); 271 | register_->align(*aligned, est_T_l2_m.cast()); 272 | if (!register_->hasConverged()) 273 | { 274 | cout << "register cant converge, please check initial value !!!" << endl; 275 | return; 276 | } 277 | Eigen::Matrix4d T_l2_m = (register_->getFinalTransformation()).cast(); 278 | 279 | // update lidar frame 280 | aligned2.first.gT = T_l2_m; 281 | Eigen::Matrix4d T_l1_m = aligned1.first.gT; 282 | aligned2.first.T = T_l1_m.inverse() * T_l2_m; 283 | 284 | // update local map 285 | *local_map_ += *aligned; 286 | } 287 | 288 | // generate constraints 289 | vector> corres; 290 | for (int i = 1; i < aligned_lidar_imu_buffer_.size(); i++) 291 | { 292 | // get neighbor frame 293 | const auto &aligned1 = aligned_lidar_imu_buffer_[i - 1]; 294 | const auto &aligned2 = aligned_lidar_imu_buffer_[i]; 295 | 296 | // calculate relative transform between neighbor lidar 297 | Eigen::Quaterniond q_l2_l1 = Eigen::Quaterniond(aligned2.first.T.block<3, 3>(0, 0)); 298 | 299 | // calculate relative transform between neighbor interpolated imu 300 | Eigen::Quaterniond q_b1_w = aligned1.second; 301 | Eigen::Quaterniond q_b2_w = aligned2.second; 302 | Eigen::Quaterniond q_b2_b1 = q_b1_w.inverse() * q_b2_w; 303 | 304 | corres.push_back(move(pair(q_l2_l1, q_b2_b1))); 305 | corres2_ = corres; 306 | } 307 | 308 | Eigen::Quaterniond result = solve(corres); 309 | result.normalize(); 310 | 311 | // check whether optimize fail 312 | // double angle = fabs(q_l_b_.angularDistance(result)); 313 | // if (angle > 0.5236) 314 | // { 315 | // cout << "the difference between before and after optimze is " << angle << " which greater than given threshold 0.5236 !!!" << endl; 316 | // return; 317 | // } 318 | q_l_b_ = result; 319 | } 320 | 321 | Eigen::Vector3d CalibExRLidarImu::calib(bool integration) 322 | { 323 | if (lidar_buffer_.size() == 0 || imu_buffer_.size() == 0) 324 | { 325 | cout << "no lidar data or imu data !!!" << endl; 326 | return init_R_; 327 | } 328 | 329 | cout << "total lidar buffer size " << lidar_buffer_.size() << ", imu buffer size " << imu_buffer_.size() << endl; 330 | 331 | // integration rotation of imu, when raw imu attitude has big error 332 | if (integration) 333 | { 334 | imu_buffer_[0].rot = Eigen::Quaterniond::Identity(); 335 | for (int i = 1; i < imu_buffer_.size(); i++) 336 | { 337 | Eigen::Vector3d bar_gyr = 0.5 * (imu_buffer_[i - 1].gyr + imu_buffer_[i].gyr); 338 | Eigen::Vector3d angle_inc = bar_gyr * (imu_buffer_[i].stamp - imu_buffer_[i - 1].stamp); 339 | Eigen::Quaterniond rot_inc = Eigen::Quaterniond(1.0, 0.5 * angle_inc[0], 0.5 * angle_inc[1], 0.5 * angle_inc[2]); 340 | imu_buffer_[i].rot = imu_buffer_[i - 1].rot * rot_inc; 341 | } 342 | } 343 | 344 | // move invalid lidar frame which got before first imu frame 345 | auto invalid_lidar_it = lidar_buffer_.begin(); 346 | for (; invalid_lidar_it != lidar_buffer_.end(); invalid_lidar_it++) 347 | { 348 | if (invalid_lidar_it->stamp >= imu_buffer_[0].stamp) 349 | break; 350 | } 351 | if (invalid_lidar_it != lidar_buffer_.begin()) 352 | lidar_buffer_.erase(lidar_buffer_.begin(), invalid_lidar_it); 353 | if (lidar_buffer_.size() == 0) 354 | { 355 | cout << "no valid lidar frame !!!" << endl; 356 | return move(Eigen::Vector3d(0.0, 0.0, 0.0)); 357 | } 358 | 359 | // get time-aligned lidar odometry rotation and imu integration rotation 360 | auto last_imu_it = imu_buffer_.begin(); 361 | for (int i = 0; i < lidar_buffer_.size(); i++) 362 | { 363 | // get lidar information 364 | const auto &lidar_frame = lidar_buffer_[i]; 365 | 366 | // get last imu frame which before current lidar frame 367 | for (; last_imu_it != imu_buffer_.end(); last_imu_it++) 368 | { 369 | if (last_imu_it->stamp >= lidar_frame.stamp) 370 | break; 371 | } 372 | if (last_imu_it != imu_buffer_.begin()) 373 | last_imu_it--; 374 | 375 | // get interpolated imu attitude at lidar stamp 376 | auto imu_it1 = last_imu_it; 377 | auto imu_it2 = last_imu_it + 1; 378 | if (imu_buffer_.end() == imu_it2) 379 | break; 380 | assert(imu_it2->stamp >= lidar_frame.stamp || imu_it1->stamp < imu_it2->stamp); // this shouldnt happen 381 | Eigen::Quaterniond q_b1_w = imu_it1->rot; 382 | Eigen::Quaterniond q_b2_w = imu_it2->rot; 383 | double scale = (lidar_frame.stamp - imu_it1->stamp) / (imu_it2->stamp - imu_it1->stamp); 384 | Eigen::Quaterniond q_inter_w = getInterpolatedAttitude(q_b1_w, q_b2_w, scale); 385 | 386 | // buffer aligned information 387 | aligned_lidar_imu_buffer_.push_back(move(pair(lidar_frame, q_inter_w))); 388 | } 389 | 390 | // solve initial transform between lidar and imu 391 | vector> corres(0); 392 | for (int i = 1; i < aligned_lidar_imu_buffer_.size(); i++) 393 | { 394 | // get neighbor aligned frame 395 | const auto &aligned1 = aligned_lidar_imu_buffer_[i - 1]; 396 | const auto &aligned2 = aligned_lidar_imu_buffer_[i]; 397 | 398 | // get initial relative transform between neighbor lidar 399 | Eigen::Quaterniond q_l2_l1 = Eigen::Quaterniond(aligned_lidar_imu_buffer_[i].first.T.block<3, 3>(0, 0)); 400 | 401 | // calculate relative transform between neighbor interpolated imu 402 | Eigen::Quaterniond q_b1_w = aligned1.second; 403 | Eigen::Quaterniond q_b2_w = aligned2.second; 404 | Eigen::Quaterniond q_b2_b1 = q_b1_w.inverse() * q_b2_w; 405 | 406 | corres.push_back(move(pair(q_l2_l1, q_b2_b1))); 407 | corres1_ = corres; 408 | } 409 | q_l_b_ = solve(corres); 410 | 411 | // optimize: use initial result to estimate transform between neighbor lidar frame for improving matching precise 412 | optimize(); 413 | 414 | // get result 415 | tf::Matrix3x3 mat(tf::Quaternion(q_l_b_.x(), q_l_b_.y(), q_l_b_.z(), q_l_b_.w())); 416 | double roll, pitch, yaw; 417 | mat.getRPY(roll, pitch, yaw); 418 | 419 | return move(Eigen::Vector3d(roll, pitch, yaw)); 420 | } 421 | -------------------------------------------------------------------------------- /ndt_omp/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: Empty 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: false 47 | ColumnLimit: 256 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 0 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Delimiter: pb 97 | Language: TextProto 98 | BasedOnStyle: google 99 | ReflowComments: true 100 | SortIncludes: false 101 | SortUsingDeclarations: false 102 | SpaceAfterCStyleCast: false 103 | SpaceAfterTemplateKeyword: false 104 | SpaceBeforeAssignmentOperators: true 105 | SpaceBeforeParens: Never 106 | SpaceInEmptyParentheses: false 107 | SpacesBeforeTrailingComments: 2 108 | SpacesInAngles: false 109 | SpacesInContainerLiterals: true 110 | SpacesInCStyleCastParentheses: false 111 | SpacesInParentheses: false 112 | SpacesInSquareBrackets: false 113 | Standard: Auto 114 | TabWidth: 8 115 | UseTab: Never 116 | ... 117 | 118 | -------------------------------------------------------------------------------- /ndt_omp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ndt_omp) 3 | 4 | # should use march=native ? 5 | add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 6 | set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 7 | 8 | # pcl 1.7 causes a segfault when it is built with debug mode 9 | set(CMAKE_BUILD_TYPE "RELEASE") 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | pcl_ros 14 | ) 15 | 16 | find_package(PCL 1.7 REQUIRED) 17 | include_directories(${PCL_INCLUDE_DIRS}) 18 | link_directories(${PCL_LIBRARY_DIRS}) 19 | add_definitions(${PCL_DEFINITIONS}) 20 | 21 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 22 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 23 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 24 | 25 | find_package(OpenMP) 26 | if (OPENMP_FOUND) 27 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 28 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 29 | endif() 30 | 31 | ################################### 32 | ## catkin specific configuration ## 33 | ################################### 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | LIBRARIES ndt_omp 37 | ) 38 | 39 | ########### 40 | ## Build ## 41 | ########### 42 | include_directories(include) 43 | include_directories( 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | add_library(ndt_omp 48 | src/pclomp/voxel_grid_covariance_omp.cpp 49 | src/pclomp/ndt_omp.cpp 50 | src/pclomp/gicp_omp.cpp 51 | ) 52 | 53 | add_executable(align 54 | apps/align.cpp 55 | ) 56 | add_dependencies(align 57 | ndt_omp 58 | ) 59 | target_link_libraries(align 60 | ${catkin_LIBRARIES} 61 | ${PCL_LIBRARIES} 62 | ndt_omp 63 | ) 64 | 65 | ############ 66 | ## INSTAL ## 67 | ############ 68 | 69 | install( 70 | TARGETS 71 | ndt_omp 72 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 74 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | ) 76 | 77 | # install headers 78 | install(DIRECTORY include/pclomp 79 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 80 | -------------------------------------------------------------------------------- /ndt_omp/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, k.koide 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /ndt_omp/README.md: -------------------------------------------------------------------------------- 1 | # ndt_omp 2 | This package provides an OpenMP-boosted Normal Distributions Transform (and GICP) algorithm derived from pcl. The NDT algorithm is modified to be SSE-friendly and multi-threaded. It can run up to 10 times faster than its original version in pcl. 3 | 4 | ### Benchmark (on Core i7-6700K) 5 | ``` 6 | $ roscd ndt_omp/data 7 | $ rosrun ndt_omp align 251370668.pcd 251371071.pcd 8 | --- pcl::NDT --- 9 | single : 282.222[msec] 10 | 10times: 2921.92[msec] 11 | fitness: 0.213937 12 | 13 | --- pclomp::NDT (KDTREE, 1 threads) --- 14 | single : 207.697[msec] 15 | 10times: 2059.19[msec] 16 | fitness: 0.213937 17 | 18 | --- pclomp::NDT (DIRECT7, 1 threads) --- 19 | single : 139.433[msec] 20 | 10times: 1356.79[msec] 21 | fitness: 0.214205 22 | 23 | --- pclomp::NDT (DIRECT1, 1 threads) --- 24 | single : 34.6418[msec] 25 | 10times: 317.03[msec] 26 | fitness: 0.208511 27 | 28 | --- pclomp::NDT (KDTREE, 8 threads) --- 29 | single : 54.9903[msec] 30 | 10times: 500.51[msec] 31 | fitness: 0.213937 32 | 33 | --- pclomp::NDT (DIRECT7, 8 threads) --- 34 | single : 63.1442[msec] 35 | 10times: 343.336[msec] 36 | fitness: 0.214205 37 | 38 | --- pclomp::NDT (DIRECT1, 8 threads) --- 39 | single : 17.2353[msec] 40 | 10times: 100.025[msec] 41 | fitness: 0.208511 42 | ``` 43 | 44 | Several methods for neighbor voxel search are implemented. If you select pclomp::KDTREE, results will be completely same as the original pcl::NDT. We recommend to use pclomp::DIRECT7 which is faster and stable. If you need extremely fast registration, choose pclomp::DIRECT1, but it might be a bit unstable. 45 | 46 |
47 | Red: target, Green: source, Blue: aligned 48 | 49 | ## Related packages 50 | - [ndt_omp](https://github.com/koide3/ndt_omp) 51 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 52 | -------------------------------------------------------------------------------- /ndt_omp/apps/align.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | // align point clouds and measure processing time 15 | pcl::PointCloud::Ptr align(pcl::Registration::Ptr registration, const pcl::PointCloud::Ptr& target_cloud, const pcl::PointCloud::Ptr& source_cloud ) { 16 | registration->setInputTarget(target_cloud); 17 | registration->setInputSource(source_cloud); 18 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 19 | 20 | auto t1 = ros::WallTime::now(); 21 | registration->align(*aligned); 22 | auto t2 = ros::WallTime::now(); 23 | std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl; 24 | 25 | for(int i=0; i<10; i++) { 26 | registration->align(*aligned); 27 | } 28 | auto t3 = ros::WallTime::now(); 29 | std::cout << "10times: " << (t3 - t2).toSec() * 1000 << "[msec]" << std::endl; 30 | std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl; 31 | 32 | return aligned; 33 | } 34 | 35 | 36 | int main(int argc, char** argv) { 37 | if(argc != 3) { 38 | std::cout << "usage: align target.pcd source.pcd" << std::endl; 39 | return 0; 40 | } 41 | 42 | std::string target_pcd = argv[1]; 43 | std::string source_pcd = argv[2]; 44 | 45 | pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud()); 46 | pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud()); 47 | 48 | if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) { 49 | std::cerr << "failed to load " << target_pcd << std::endl; 50 | return 0; 51 | } 52 | if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) { 53 | std::cerr << "failed to load " << source_pcd << std::endl; 54 | return 0; 55 | } 56 | 57 | // downsampling 58 | pcl::PointCloud::Ptr downsampled(new pcl::PointCloud()); 59 | 60 | pcl::VoxelGrid voxelgrid; 61 | voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f); 62 | 63 | voxelgrid.setInputCloud(target_cloud); 64 | voxelgrid.filter(*downsampled); 65 | *target_cloud = *downsampled; 66 | 67 | voxelgrid.setInputCloud(source_cloud); 68 | voxelgrid.filter(*downsampled); 69 | source_cloud = downsampled; 70 | 71 | ros::Time::init(); 72 | 73 | // benchmark 74 | std::cout << "--- pcl::GICP ---" << std::endl; 75 | pcl::GeneralizedIterativeClosestPoint::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint()); 76 | pcl::PointCloud::Ptr aligned = align(gicp, target_cloud, source_cloud); 77 | 78 | std::cout << "--- pclomp::GICP ---" << std::endl; 79 | pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint()); 80 | aligned = align(gicp_omp, target_cloud, source_cloud); 81 | 82 | 83 | std::cout << "--- pcl::NDT ---" << std::endl; 84 | pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); 85 | ndt->setResolution(1.0); 86 | aligned = align(ndt, target_cloud, source_cloud); 87 | 88 | std::vector num_threads = {1, omp_get_max_threads()}; 89 | std::vector> search_methods = { 90 | {"KDTREE", pclomp::KDTREE}, 91 | {"DIRECT7", pclomp::DIRECT7}, 92 | {"DIRECT1", pclomp::DIRECT1} 93 | }; 94 | 95 | pclomp::NormalDistributionsTransform::Ptr ndt_omp(new pclomp::NormalDistributionsTransform()); 96 | ndt_omp->setResolution(1.0); 97 | 98 | for(int n : num_threads) { 99 | for(const auto& search_method : search_methods) { 100 | std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---" << std::endl; 101 | ndt_omp->setNumThreads(n); 102 | ndt_omp->setNeighborhoodSearchMethod(search_method.second); 103 | aligned = align(ndt_omp, target_cloud, source_cloud); 104 | } 105 | } 106 | 107 | // visulization 108 | pcl::visualization::PCLVisualizer vis("vis"); 109 | pcl::visualization::PointCloudColorHandlerCustom target_handler(target_cloud, 255.0, 0.0, 0.0); 110 | pcl::visualization::PointCloudColorHandlerCustom source_handler(source_cloud, 0.0, 255.0, 0.0); 111 | pcl::visualization::PointCloudColorHandlerCustom aligned_handler(aligned, 0.0, 0.0, 255.0); 112 | vis.addPointCloud(target_cloud, target_handler, "target"); 113 | vis.addPointCloud(source_cloud, source_handler, "source"); 114 | vis.addPointCloud(aligned, aligned_handler, "aligned"); 115 | vis.spin(); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /ndt_omp/data/251370668.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chennuo0125-HIT/lidar_imu_calib/6b578685aeb10de9f0e419444d2cc3e5f420e648/ndt_omp/data/251370668.pcd -------------------------------------------------------------------------------- /ndt_omp/data/251371071.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chennuo0125-HIT/lidar_imu_calib/6b578685aeb10de9f0e419444d2cc3e5f420e648/ndt_omp/data/251371071.pcd -------------------------------------------------------------------------------- /ndt_omp/data/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chennuo0125-HIT/lidar_imu_calib/6b578685aeb10de9f0e419444d2cc3e5f420e648/ndt_omp/data/screenshot.png -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/gicp_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_GICP_OMP_H_ 42 | #define PCL_GICP_OMP_H_ 43 | 44 | #include 45 | #include 46 | 47 | namespace pclomp 48 | { 49 | /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the 50 | * generalized iterative closest point algorithm as described by Alex Segal et al. in 51 | * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf 52 | * The approach is based on using anistropic cost functions to optimize the alignment 53 | * after closest point assignments have been made. 54 | * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and 55 | * FLANN. 56 | * \author Nizar Sallem 57 | * \ingroup registration 58 | */ 59 | template 60 | class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint 61 | { 62 | public: 63 | using pcl::IterativeClosestPoint::reg_name_; 64 | using pcl::IterativeClosestPoint::getClassName; 65 | using pcl::IterativeClosestPoint::indices_; 66 | using pcl::IterativeClosestPoint::target_; 67 | using pcl::IterativeClosestPoint::input_; 68 | using pcl::IterativeClosestPoint::tree_; 69 | using pcl::IterativeClosestPoint::tree_reciprocal_; 70 | using pcl::IterativeClosestPoint::nr_iterations_; 71 | using pcl::IterativeClosestPoint::max_iterations_; 72 | using pcl::IterativeClosestPoint::previous_transformation_; 73 | using pcl::IterativeClosestPoint::final_transformation_; 74 | using pcl::IterativeClosestPoint::transformation_; 75 | using pcl::IterativeClosestPoint::transformation_epsilon_; 76 | using pcl::IterativeClosestPoint::converged_; 77 | using pcl::IterativeClosestPoint::corr_dist_threshold_; 78 | using pcl::IterativeClosestPoint::inlier_threshold_; 79 | using pcl::IterativeClosestPoint::min_number_correspondences_; 80 | using pcl::IterativeClosestPoint::update_visualizer_; 81 | 82 | using PointCloudSource = pcl::PointCloud; 83 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 84 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 85 | 86 | using PointCloudTarget = pcl::PointCloud; 87 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 88 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 89 | 90 | using PointIndicesPtr = pcl::PointIndices::Ptr; 91 | using PointIndicesConstPtr = pcl::PointIndices::ConstPtr; 92 | 93 | using MatricesVector = std::vector >; 94 | using MatricesVectorPtr = boost::shared_ptr; 95 | using MatricesVectorConstPtr = boost::shared_ptr; 96 | 97 | using InputKdTree = typename pcl::Registration::KdTree; 98 | using InputKdTreePtr = typename pcl::Registration::KdTreePtr; 99 | 100 | using Ptr = boost::shared_ptr >; 101 | using ConstPtr = boost::shared_ptr >; 102 | 103 | 104 | using Vector6d = Eigen::Matrix; 105 | 106 | /** \brief Empty constructor. */ 107 | GeneralizedIterativeClosestPoint () 108 | : k_correspondences_(20) 109 | , gicp_epsilon_(0.001) 110 | , rotation_epsilon_(2e-3) 111 | , mahalanobis_(0) 112 | , max_inner_iterations_(20) 113 | { 114 | min_number_correspondences_ = 4; 115 | reg_name_ = "GeneralizedIterativeClosestPoint"; 116 | max_iterations_ = 200; 117 | transformation_epsilon_ = 5e-4; 118 | corr_dist_threshold_ = 5.; 119 | rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src, 120 | const std::vector& indices_src, 121 | const PointCloudTarget& cloud_tgt, 122 | const std::vector& indices_tgt, 123 | Eigen::Matrix4f& transformation_matrix) 124 | { 125 | estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 126 | }; 127 | } 128 | 129 | /** \brief Provide a pointer to the input dataset 130 | * \param cloud the const boost shared pointer to a PointCloud message 131 | */ 132 | inline void 133 | setInputSource (const PointCloudSourceConstPtr &cloud) override 134 | { 135 | 136 | if (cloud->points.empty ()) 137 | { 138 | PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 139 | return; 140 | } 141 | PointCloudSource input = *cloud; 142 | // Set all the point.data[3] values to 1 to aid the rigid transformation 143 | for (size_t i = 0; i < input.size (); ++i) 144 | input[i].data[3] = 1.0; 145 | 146 | pcl::IterativeClosestPoint::setInputSource (cloud); 147 | input_covariances_.reset (); 148 | } 149 | 150 | /** \brief Provide a pointer to the covariances of the input source (if computed externally!). 151 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 152 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 153 | * \param[in] target the input point cloud target 154 | */ 155 | inline void 156 | setSourceCovariances (const MatricesVectorPtr& covariances) 157 | { 158 | input_covariances_ = covariances; 159 | } 160 | 161 | /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) 162 | * \param[in] target the input point cloud target 163 | */ 164 | inline void 165 | setInputTarget (const PointCloudTargetConstPtr &target) override 166 | { 167 | pcl::IterativeClosestPoint::setInputTarget(target); 168 | target_covariances_.reset (); 169 | } 170 | 171 | /** \brief Provide a pointer to the covariances of the input target (if computed externally!). 172 | * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. 173 | * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). 174 | * \param[in] target the input point cloud target 175 | */ 176 | inline void 177 | setTargetCovariances (const MatricesVectorPtr& covariances) 178 | { 179 | target_covariances_ = covariances; 180 | } 181 | 182 | /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative 183 | * non-linear Levenberg-Marquardt approach. 184 | * \param[in] cloud_src the source point cloud dataset 185 | * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 186 | * \param[in] cloud_tgt the target point cloud dataset 187 | * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src 188 | * \param[out] transformation_matrix the resultant transformation matrix 189 | */ 190 | void 191 | estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 192 | const std::vector &indices_src, 193 | const PointCloudTarget &cloud_tgt, 194 | const std::vector &indices_tgt, 195 | Eigen::Matrix4f &transformation_matrix); 196 | 197 | /** \brief \return Mahalanobis distance matrix for the given point index */ 198 | inline const Eigen::Matrix4f& mahalanobis(size_t index) const 199 | { 200 | assert(index < mahalanobis_.size()); 201 | return mahalanobis_[index]; 202 | } 203 | 204 | /** \brief Computes rotation matrix derivative. 205 | * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5] 206 | * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] 207 | * param x array representing 3D transformation 208 | * param R rotation matrix 209 | * param g gradient vector 210 | */ 211 | void 212 | computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; 213 | 214 | /** \brief Set the rotation epsilon (maximum allowable difference between two 215 | * consecutive rotations) in order for an optimization to be considered as having 216 | * converged to the final solution. 217 | * \param epsilon the rotation epsilon 218 | */ 219 | inline void 220 | setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } 221 | 222 | /** \brief Get the rotation epsilon (maximum allowable difference between two 223 | * consecutive rotations) as set by the user. 224 | */ 225 | inline double 226 | getRotationEpsilon () { return (rotation_epsilon_); } 227 | 228 | /** \brief Set the number of neighbors used when selecting a point neighbourhood 229 | * to compute covariances. 230 | * A higher value will bring more accurate covariance matrix but will make 231 | * covariances computation slower. 232 | * \param k the number of neighbors to use when computing covariances 233 | */ 234 | void 235 | setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 236 | 237 | /** \brief Get the number of neighbors used when computing covariances as set by 238 | * the user 239 | */ 240 | int 241 | getCorrespondenceRandomness () { return (k_correspondences_); } 242 | 243 | /** set maximum number of iterations at the optimization step 244 | * \param[in] max maximum number of iterations for the optimizer 245 | */ 246 | void 247 | setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } 248 | 249 | ///\return maximum number of iterations at the optimization step 250 | int 251 | getMaximumOptimizerIterations () { return (max_inner_iterations_); } 252 | 253 | protected: 254 | 255 | /** \brief The number of neighbors used for covariances computation. 256 | * default: 20 257 | */ 258 | int k_correspondences_; 259 | 260 | /** \brief The epsilon constant for gicp paper; this is NOT the convergence 261 | * tolerance 262 | * default: 0.001 263 | */ 264 | double gicp_epsilon_; 265 | 266 | /** The epsilon constant for rotation error. (In GICP the transformation epsilon 267 | * is split in rotation part and translation part). 268 | * default: 2e-3 269 | */ 270 | double rotation_epsilon_; 271 | 272 | /** \brief base transformation */ 273 | Eigen::Matrix4f base_transformation_; 274 | 275 | /** \brief Temporary pointer to the source dataset. */ 276 | const PointCloudSource *tmp_src_; 277 | 278 | /** \brief Temporary pointer to the target dataset. */ 279 | const PointCloudTarget *tmp_tgt_; 280 | 281 | /** \brief Temporary pointer to the source dataset indices. */ 282 | const std::vector *tmp_idx_src_; 283 | 284 | /** \brief Temporary pointer to the target dataset indices. */ 285 | const std::vector *tmp_idx_tgt_; 286 | 287 | 288 | /** \brief Input cloud points covariances. */ 289 | MatricesVectorPtr input_covariances_; 290 | 291 | /** \brief Target cloud points covariances. */ 292 | MatricesVectorPtr target_covariances_; 293 | 294 | /** \brief Mahalanobis matrices holder. */ 295 | std::vector mahalanobis_; 296 | 297 | /** \brief maximum number of optimizations */ 298 | int max_inner_iterations_; 299 | 300 | /** \brief compute points covariances matrices according to the K nearest 301 | * neighbors. K is set via setCorrespondenceRandomness() method. 302 | * \param cloud pointer to point cloud 303 | * \param tree KD tree performer for nearest neighbors search 304 | * \param[out] cloud_covariances covariances matrices for each point in the cloud 305 | */ 306 | template 307 | void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, 308 | const typename pcl::search::KdTree::ConstPtr tree, 309 | MatricesVector& cloud_covariances); 310 | 311 | /** \return trace of mat1^t . mat2 312 | * \param mat1 matrix of dimension nxm 313 | * \param mat2 matrix of dimension nxp 314 | */ 315 | inline double 316 | matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const 317 | { 318 | double r = 0.; 319 | size_t n = mat1.rows(); 320 | // tr(mat1^t.mat2) 321 | for(size_t i = 0; i < n; i++) 322 | for(size_t j = 0; j < n; j++) 323 | r += mat1 (j, i) * mat2 (i,j); 324 | return r; 325 | } 326 | 327 | /** \brief Rigid transformation computation method with initial guess. 328 | * \param output the transformed input point cloud dataset using the rigid transformation found 329 | * \param guess the initial guess of the transformation to compute 330 | */ 331 | void 332 | computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; 333 | 334 | /** \brief Search for the closest nearest neighbor of a given point. 335 | * \param query the point to search a nearest neighbour for 336 | * \param index vector of size 1 to store the index of the nearest neighbour found 337 | * \param distance vector of size 1 to store the distance to nearest neighbour found 338 | */ 339 | inline bool 340 | searchForNeighbors (const PointSource &query, std::vector& index, std::vector& distance) const 341 | { 342 | int k = tree_->nearestKSearch (query, 1, index, distance); 343 | if (k == 0) 344 | return (false); 345 | return (true); 346 | } 347 | 348 | /// \brief compute transformation matrix from transformation matrix 349 | void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; 350 | 351 | /// \brief optimization functor structure 352 | struct OptimizationFunctorWithIndices : public BFGSDummyFunctor 353 | { 354 | OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp) 355 | : BFGSDummyFunctor (), gicp_(gicp) {} 356 | double operator() (const Vector6d& x) override; 357 | void df(const Vector6d &x, Vector6d &df) override; 358 | void fdf(const Vector6d &x, double &f, Vector6d &df) override; 359 | 360 | const GeneralizedIterativeClosestPoint *gicp_; 361 | }; 362 | 363 | std::function &cloud_src, 364 | const std::vector &src_indices, 365 | const pcl::PointCloud &cloud_tgt, 366 | const std::vector &tgt_indices, 367 | Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_; 368 | }; 369 | } 370 | 371 | #endif //#ifndef PCL_GICP_H_ 372 | -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/gicp_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | #ifndef PCL_REGISTRATION_IMPL_GICP_OMP_HPP_ 41 | #define PCL_REGISTRATION_IMPL_GICP_OMP_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | //////////////////////////////////////////////////////////////////////////////////////// 48 | template 49 | template void 50 | pclomp::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, 51 | const typename pcl::search::KdTree::ConstPtr kdtree, 52 | MatricesVector& cloud_covariances) 53 | { 54 | if (k_correspondences_ > int (cloud->size ())) 55 | { 56 | PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_); 57 | return; 58 | } 59 | 60 | // We should never get there but who knows 61 | if(cloud_covariances.size () < cloud->size ()) 62 | cloud_covariances.resize (cloud->size ()); 63 | 64 | std::vector> nn_indecies_array(omp_get_max_threads()); 65 | std::vector> nn_dist_sq_array(omp_get_max_threads()); 66 | 67 | #pragma omp parallel for 68 | for(int i=0; isize(); i++) { 69 | auto& nn_indecies = nn_indecies_array[omp_get_thread_num()]; 70 | auto& nn_dist_sq = nn_dist_sq_array[omp_get_thread_num()]; 71 | 72 | const PointT &query_point = cloud->at(i); 73 | Eigen::Vector3d mean = Eigen::Vector3d::Zero(); 74 | Eigen::Matrix3d &cov = cloud_covariances[i]; 75 | // Zero out the cov and mean 76 | cov.setZero (); 77 | 78 | // Search for the K nearest neighbours 79 | kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq); 80 | 81 | // Find the covariance matrix 82 | for(int j = 0; j < k_correspondences_; j++) { 83 | const PointT &pt = (*cloud)[nn_indecies[j]]; 84 | 85 | mean[0] += pt.x; 86 | mean[1] += pt.y; 87 | mean[2] += pt.z; 88 | 89 | cov(0,0) += pt.x*pt.x; 90 | 91 | cov(1,0) += pt.y*pt.x; 92 | cov(1,1) += pt.y*pt.y; 93 | 94 | cov(2,0) += pt.z*pt.x; 95 | cov(2,1) += pt.z*pt.y; 96 | cov(2,2) += pt.z*pt.z; 97 | } 98 | 99 | mean /= static_cast (k_correspondences_); 100 | // Get the actual covariance 101 | for (int k = 0; k < 3; k++) 102 | for (int l = 0; l <= k; l++) 103 | { 104 | cov(k,l) /= static_cast (k_correspondences_); 105 | cov(k,l) -= mean[k]*mean[l]; 106 | cov(l,k) = cov(k,l); 107 | } 108 | 109 | // Compute the SVD (covariance matrix is symmetric so U = V') 110 | Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); 111 | cov.setZero (); 112 | Eigen::Matrix3d U = svd.matrixU (); 113 | // Reconstitute the covariance matrix with modified singular values using the column // vectors in V. 114 | for(int k = 0; k < 3; k++) { 115 | Eigen::Vector3d col = U.col(k); 116 | double v = 1.; // biggest 2 singular values replaced by 1 117 | if(k == 2) // smallest singular value replaced by gicp_epsilon 118 | v = gicp_epsilon_; 119 | cov+= v * col * col.transpose(); 120 | } 121 | } 122 | } 123 | 124 | //////////////////////////////////////////////////////////////////////////////////////// 125 | template void 126 | pclomp::GeneralizedIterativeClosestPoint::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const 127 | { 128 | Eigen::Matrix3d dR_dPhi; 129 | Eigen::Matrix3d dR_dTheta; 130 | Eigen::Matrix3d dR_dPsi; 131 | 132 | double phi = x[3], theta = x[4], psi = x[5]; 133 | 134 | double cphi = std::cos(phi), sphi = sin(phi); 135 | double ctheta = std::cos(theta), stheta = sin(theta); 136 | double cpsi = std::cos(psi), spsi = sin(psi); 137 | 138 | dR_dPhi(0,0) = 0.; 139 | dR_dPhi(1,0) = 0.; 140 | dR_dPhi(2,0) = 0.; 141 | 142 | dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta; 143 | dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta; 144 | dR_dPhi(2,1) = cphi*ctheta; 145 | 146 | dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta; 147 | dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta; 148 | dR_dPhi(2,2) = -ctheta*sphi; 149 | 150 | dR_dTheta(0,0) = -cpsi*stheta; 151 | dR_dTheta(1,0) = -spsi*stheta; 152 | dR_dTheta(2,0) = -ctheta; 153 | 154 | dR_dTheta(0,1) = cpsi*ctheta*sphi; 155 | dR_dTheta(1,1) = ctheta*sphi*spsi; 156 | dR_dTheta(2,1) = -sphi*stheta; 157 | 158 | dR_dTheta(0,2) = cphi*cpsi*ctheta; 159 | dR_dTheta(1,2) = cphi*ctheta*spsi; 160 | dR_dTheta(2,2) = -cphi*stheta; 161 | 162 | dR_dPsi(0,0) = -ctheta*spsi; 163 | dR_dPsi(1,0) = cpsi*ctheta; 164 | dR_dPsi(2,0) = 0.; 165 | 166 | dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta; 167 | dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta; 168 | dR_dPsi(2,1) = 0.; 169 | 170 | dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta; 171 | dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta; 172 | dR_dPsi(2,2) = 0.; 173 | 174 | g[3] = matricesInnerProd(dR_dPhi, R); 175 | g[4] = matricesInnerProd(dR_dTheta, R); 176 | g[5] = matricesInnerProd(dR_dPsi, R); 177 | } 178 | 179 | //////////////////////////////////////////////////////////////////////////////////////// 180 | template void 181 | pclomp::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 182 | const std::vector &indices_src, 183 | const PointCloudTarget &cloud_tgt, 184 | const std::vector &indices_tgt, 185 | Eigen::Matrix4f &transformation_matrix) 186 | { 187 | if (indices_src.size () < 4) // need at least 4 samples 188 | { 189 | PCL_THROW_EXCEPTION (pcl::NotEnoughPointsException, 190 | "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!"); 191 | return; 192 | } 193 | // Set the initial solution 194 | Vector6d x = Vector6d::Zero (); 195 | x[0] = transformation_matrix (0,3); 196 | x[1] = transformation_matrix (1,3); 197 | x[2] = transformation_matrix (2,3); 198 | x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2)); 199 | x[4] = asin (-transformation_matrix (2,0)); 200 | x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0)); 201 | 202 | // Set temporary pointers 203 | tmp_src_ = &cloud_src; 204 | tmp_tgt_ = &cloud_tgt; 205 | tmp_idx_src_ = &indices_src; 206 | tmp_idx_tgt_ = &indices_tgt; 207 | 208 | // Optimize using forward-difference approximation LM 209 | const double gradient_tol = 1e-2; 210 | OptimizationFunctorWithIndices functor(this); 211 | BFGS bfgs (functor); 212 | bfgs.parameters.sigma = 0.01; 213 | bfgs.parameters.rho = 0.01; 214 | bfgs.parameters.tau1 = 9; 215 | bfgs.parameters.tau2 = 0.05; 216 | bfgs.parameters.tau3 = 0.5; 217 | bfgs.parameters.order = 3; 218 | 219 | int inner_iterations_ = 0; 220 | int result = bfgs.minimizeInit (x); 221 | result = BFGSSpace::Running; 222 | do 223 | { 224 | inner_iterations_++; 225 | result = bfgs.minimizeOneStep (x); 226 | if(result) 227 | { 228 | break; 229 | } 230 | result = bfgs.testGradient(gradient_tol); 231 | } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_); 232 | if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_) 233 | { 234 | PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]"); 235 | PCL_DEBUG ("BFGS solver finished with exit code %i \n", result); 236 | transformation_matrix.setIdentity(); 237 | applyState(transformation_matrix, x); 238 | } 239 | else 240 | PCL_THROW_EXCEPTION(pcl::SolverDidntConvergeException, 241 | "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); 242 | } 243 | 244 | //////////////////////////////////////////////////////////////////////////////////////// 245 | template inline double 246 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator() (const Vector6d& x) 247 | { 248 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 249 | gicp_->applyState(transformation_matrix, x); 250 | double f = 0; 251 | std::vector f_array(omp_get_max_threads(), 0.0); 252 | 253 | int m = static_cast (gicp_->tmp_idx_src_->size ()); 254 | #pragma omp parallel for 255 | for(int i = 0; i < m; ++i) 256 | { 257 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 258 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap(); 259 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 260 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); 261 | // Estimate the distance (cost function) 262 | // The last coordiante is still guaranteed to be set to 1.0 263 | // Eigen::AlignedVector3 res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); 264 | // Eigen::AlignedVector3 temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res); 265 | Eigen::Vector4f res = transformation_matrix * p_src - p_tgt; 266 | Eigen::Matrix4f maha = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]); 267 | // Eigen::Vector4d temp(maha * res); 268 | // increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes) 269 | // double ret = double(res.transpose() * temp); 270 | double ret = res.dot(maha*res); 271 | f_array[omp_get_thread_num()] += ret; 272 | } 273 | f = std::accumulate(f_array.begin(), f_array.end(), 0.0); 274 | return f/m; 275 | } 276 | 277 | //////////////////////////////////////////////////////////////////////////////////////// 278 | template inline void 279 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g) 280 | { 281 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 282 | gicp_->applyState(transformation_matrix, x); 283 | //Eigen::Vector3d g_t = g.head<3> (); 284 | std::vector> R_array(omp_get_max_threads()); 285 | std::vector> g_array(omp_get_max_threads()); 286 | 287 | for (int i = 0; i < R_array.size(); i++) { 288 | R_array[i].setZero(); 289 | g_array[i].setZero(); 290 | } 291 | 292 | int m = static_cast (gicp_->tmp_idx_src_->size ()); 293 | #pragma omp parallel for 294 | for(int i = 0; i < m; ++i) 295 | { 296 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 297 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap(); 298 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 299 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); 300 | 301 | Eigen::Vector4f pp(transformation_matrix * p_src); 302 | // The last coordiante is still guaranteed to be set to 1.0 303 | Eigen::Vector4d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2], 0.0); 304 | // temp = M*res 305 | 306 | Eigen::Matrix4d maha = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]).template cast(); 307 | 308 | Eigen::Vector4d temp(maha * res); 309 | // Increment translation gradient 310 | // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) 311 | // Increment rotation gradient 312 | pp = gicp_->base_transformation_ * p_src; 313 | 314 | Eigen::Vector4d p_src3(pp[0], pp[1], pp[2], 0.0); 315 | g_array[omp_get_thread_num()] += temp; 316 | R_array[omp_get_thread_num()] += p_src3 * temp.transpose(); 317 | } 318 | 319 | g.setZero(); 320 | Eigen::Matrix4d R = Eigen::Matrix4d::Zero(); 321 | for (int i = 0; i < R_array.size(); i++) { 322 | R += R_array[i]; 323 | g.head<3>() += g_array[i].head<3>(); 324 | } 325 | 326 | g.head<3>() *= 2.0 / m; 327 | R *= 2.0 / m; 328 | 329 | gicp_->computeRDerivative(x, R.block<3, 3>(0, 0), g); 330 | } 331 | 332 | //////////////////////////////////////////////////////////////////////////////////////// 333 | template inline void 334 | pclomp::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g) 335 | { 336 | Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; 337 | gicp_->applyState(transformation_matrix, x); 338 | f = 0; 339 | g.setZero (); 340 | Eigen::Matrix3d R = Eigen::Matrix3d::Zero (); 341 | const int m = static_cast (gicp_->tmp_idx_src_->size ()); 342 | for (int i = 0; i < m; ++i) 343 | { 344 | // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp 345 | pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap (); 346 | // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp 347 | pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap (); 348 | Eigen::Vector4f pp (transformation_matrix * p_src); 349 | // The last coordinate is still guaranteed to be set to 1.0 350 | Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); 351 | // temp = M*res 352 | Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]).template block<3, 3>(0, 0).template cast() * res); 353 | // Increment total error 354 | f+= double(res.transpose() * temp); 355 | // Increment translation gradient 356 | // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) 357 | g.head<3> ()+= temp; 358 | pp = gicp_->base_transformation_ * p_src; 359 | Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]); 360 | // Increment rotation gradient 361 | R+= p_src3 * temp.transpose(); 362 | } 363 | f/= double(m); 364 | g.head<3> ()*= double(2.0/m); 365 | R*= 2.0/m; 366 | gicp_->computeRDerivative(x, R, g); 367 | } 368 | 369 | //////////////////////////////////////////////////////////////////////////////////////// 370 | template inline void 371 | pclomp::GeneralizedIterativeClosestPoint::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) 372 | { 373 | pcl::IterativeClosestPoint::initComputeReciprocal (); 374 | using namespace std; 375 | // Difference between consecutive transforms 376 | double delta = 0; 377 | // Get the size of the target 378 | const size_t N = indices_->size (); 379 | // Set the mahalanobis matrices to identity 380 | mahalanobis_.resize (N, Eigen::Matrix4f::Identity ()); 381 | // Compute target cloud covariance matrices 382 | if ((!target_covariances_) || (target_covariances_->empty ())) 383 | { 384 | target_covariances_.reset (new MatricesVector); 385 | computeCovariances (target_, tree_, *target_covariances_); 386 | } 387 | // Compute input cloud covariance matrices 388 | if ((!input_covariances_) || (input_covariances_->empty ())) 389 | { 390 | input_covariances_.reset (new MatricesVector); 391 | computeCovariances (input_, tree_reciprocal_, *input_covariances_); 392 | } 393 | 394 | base_transformation_ = Eigen::Matrix4f::Identity(); 395 | nr_iterations_ = 0; 396 | converged_ = false; 397 | double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; 398 | pcl::transformPointCloud(output, output, guess); 399 | 400 | std::vector> nn_indices_array(omp_get_max_threads()); 401 | std::vector> nn_dists_array(omp_get_max_threads()); 402 | for (auto& nn_indices : nn_indices_array) { nn_indices.resize(1); } 403 | for (auto& nn_dists : nn_dists_array) { nn_dists.resize(1); } 404 | 405 | while(!converged_) 406 | { 407 | std::atomic cnt; 408 | cnt = 0; 409 | std::vector source_indices (indices_->size ()); 410 | std::vector target_indices (indices_->size ()); 411 | 412 | // guess corresponds to base_t and transformation_ to t 413 | Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); 414 | for(size_t i = 0; i < 4; i++) 415 | for(size_t j = 0; j < 4; j++) 416 | for(size_t k = 0; k < 4; k++) 417 | transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j)); 418 | 419 | const Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> (); 420 | 421 | #pragma omp parallel for 422 | for (int i = 0; i < N; i++) 423 | { 424 | auto& nn_indices = nn_indices_array[omp_get_thread_num()]; 425 | auto& nn_dists = nn_dists_array[omp_get_thread_num()]; 426 | 427 | PointSource query = output[i]; 428 | query.getVector4fMap () = transformation_ * query.getVector4fMap (); 429 | 430 | if (!searchForNeighbors (query, nn_indices, nn_dists)) 431 | { 432 | PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]); 433 | continue; 434 | } 435 | 436 | // Check if the distance to the nearest neighbor is smaller than the user imposed threshold 437 | if (nn_dists[0] < dist_threshold) 438 | { 439 | const Eigen::Matrix3d &C1 = (*input_covariances_)[i]; 440 | const Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]]; 441 | Eigen::Matrix4f& M_ = mahalanobis_[i]; 442 | M_.setZero(); 443 | 444 | Eigen::Matrix3d M = M_.block<3, 3>(0, 0).cast(); 445 | // M = R*C1 446 | M = R * C1; 447 | // temp = M*R' + C2 = R*C1*R' + C2 448 | Eigen::Matrix3d temp = M * R.transpose(); 449 | temp+= C2; 450 | // M = temp^-1 451 | M = temp.inverse (); 452 | M_.block<3, 3>(0, 0) = M.cast(); 453 | int c = cnt++; 454 | source_indices[c] = static_cast (i); 455 | target_indices[c] = nn_indices[0]; 456 | } 457 | } 458 | // Resize to the actual number of valid correspondences 459 | source_indices.resize(cnt); target_indices.resize(cnt); 460 | 461 | std::vector> indices(source_indices.size()); 462 | for(int i = 0; i& lhs, const std::pair& rhs) { return lhs.first < rhs.first; }); 468 | 469 | for(int i = 0; i < source_indices.size(); i++) { 470 | source_indices[i] = indices[i].first; 471 | target_indices[i] = indices[i].second; 472 | } 473 | 474 | /* optimize transformation using the current assignment and Mahalanobis metrics*/ 475 | previous_transformation_ = transformation_; 476 | //optimization right here 477 | try 478 | { 479 | rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_); 480 | /* compute the delta from this iteration */ 481 | delta = 0.; 482 | for(int k = 0; k < 4; k++) { 483 | for(int l = 0; l < 4; l++) { 484 | double ratio = 1; 485 | if(k < 3 && l < 3) // rotation part of the transform 486 | ratio = 1./rotation_epsilon_; 487 | else 488 | ratio = 1./transformation_epsilon_; 489 | double c_delta = ratio*std::abs(previous_transformation_(k,l) - transformation_(k,l)); 490 | if(c_delta > delta) 491 | delta = c_delta; 492 | } 493 | } 494 | } 495 | catch (pcl::PCLException &e) 496 | { 497 | PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); 498 | break; 499 | } 500 | nr_iterations_++; 501 | // Check for convergence 502 | if (nr_iterations_ >= max_iterations_ || delta < 1) 503 | { 504 | converged_ = true; 505 | previous_transformation_ = transformation_; 506 | PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", 507 | getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); 508 | } 509 | else 510 | PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); 511 | } 512 | final_transformation_ = previous_transformation_ * guess; 513 | 514 | // Transform the point cloud 515 | pcl::transformPointCloud (*input_, output, final_transformation_); 516 | } 517 | 518 | template void 519 | pclomp::GeneralizedIterativeClosestPoint::applyState(Eigen::Matrix4f &t, const Vector6d& x) const 520 | { 521 | // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention 522 | Eigen::Matrix3f R; 523 | R = Eigen::AngleAxisf (static_cast (x[5]), Eigen::Vector3f::UnitZ ()) 524 | * Eigen::AngleAxisf (static_cast (x[4]), Eigen::Vector3f::UnitY ()) 525 | * Eigen::AngleAxisf (static_cast (x[3]), Eigen::Vector3f::UnitX ()); 526 | t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix (); 527 | Eigen::Vector4f T (static_cast (x[0]), static_cast (x[1]), static_cast (x[2]), 0.0f); 528 | t.col (3) += T; 529 | } 530 | 531 | #endif //PCL_REGISTRATION_IMPL_GICP_HPP_ 532 | -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/ndt_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2012, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id$ 38 | * 39 | */ 40 | 41 | #ifndef PCL_REGISTRATION_NDT_OMP_H_ 42 | #define PCL_REGISTRATION_NDT_OMP_H_ 43 | 44 | #include 45 | #include 46 | #include "voxel_grid_covariance_omp.h" 47 | 48 | #include 49 | 50 | namespace pclomp 51 | { 52 | enum NeighborSearchMethod { 53 | KDTREE, 54 | DIRECT26, 55 | DIRECT7, 56 | DIRECT1 57 | }; 58 | 59 | /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. 60 | * \note For more information please see 61 | * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 62 | * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 63 | * PhD thesis, Orebro University. Orebro Studies in Technology 36., 64 | * More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease 65 | * In ACM Transactions on Mathematical Software. and 66 | * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 67 | * \note Math refactored by Todor Stoyanov. 68 | * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 69 | */ 70 | template 71 | class NormalDistributionsTransform : public pcl::Registration 72 | { 73 | protected: 74 | 75 | typedef typename pcl::Registration::PointCloudSource PointCloudSource; 76 | typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 77 | typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 78 | 79 | typedef typename pcl::Registration::PointCloudTarget PointCloudTarget; 80 | typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 81 | typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 82 | 83 | typedef pcl::PointIndices::Ptr PointIndicesPtr; 84 | typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr; 85 | 86 | /** \brief Typename of searchable voxel grid containing mean and covariance. */ 87 | typedef pclomp::VoxelGridCovariance TargetGrid; 88 | /** \brief Typename of pointer to searchable voxel grid. */ 89 | typedef TargetGrid* TargetGridPtr; 90 | /** \brief Typename of const pointer to searchable voxel grid. */ 91 | typedef const TargetGrid* TargetGridConstPtr; 92 | /** \brief Typename of const pointer to searchable voxel grid leaf. */ 93 | typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; 94 | 95 | 96 | public: 97 | 98 | typedef boost::shared_ptr< NormalDistributionsTransform > Ptr; 99 | typedef boost::shared_ptr< const NormalDistributionsTransform > ConstPtr; 100 | 101 | 102 | /** \brief Constructor. 103 | * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0 104 | */ 105 | NormalDistributionsTransform(); 106 | 107 | /** \brief Empty destructor */ 108 | virtual ~NormalDistributionsTransform() {} 109 | 110 | void setNumThreads(int n) { 111 | num_threads_ = n; 112 | } 113 | 114 | /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to). 115 | * \param[in] cloud the input point cloud target 116 | */ 117 | inline void 118 | setInputTarget(const PointCloudTargetConstPtr &cloud) 119 | { 120 | pcl::Registration::setInputTarget(cloud); 121 | init(); 122 | } 123 | 124 | /** \brief Set/change the voxel grid resolution. 125 | * \param[in] resolution side length of voxels 126 | */ 127 | inline void 128 | setResolution(float resolution) 129 | { 130 | // Prevents unnessary voxel initiations 131 | if (resolution_ != resolution) 132 | { 133 | resolution_ = resolution; 134 | if (input_) 135 | init(); 136 | } 137 | } 138 | 139 | /** \brief Get voxel grid resolution. 140 | * \return side length of voxels 141 | */ 142 | inline float 143 | getResolution() const 144 | { 145 | return (resolution_); 146 | } 147 | 148 | /** \brief Get the newton line search maximum step length. 149 | * \return maximum step length 150 | */ 151 | inline double 152 | getStepSize() const 153 | { 154 | return (step_size_); 155 | } 156 | 157 | /** \brief Set/change the newton line search maximum step length. 158 | * \param[in] step_size maximum step length 159 | */ 160 | inline void 161 | setStepSize(double step_size) 162 | { 163 | step_size_ = step_size; 164 | } 165 | 166 | /** \brief Get the point cloud outlier ratio. 167 | * \return outlier ratio 168 | */ 169 | inline double 170 | getOulierRatio() const 171 | { 172 | return (outlier_ratio_); 173 | } 174 | 175 | /** \brief Set/change the point cloud outlier ratio. 176 | * \param[in] outlier_ratio outlier ratio 177 | */ 178 | inline void 179 | setOulierRatio(double outlier_ratio) 180 | { 181 | outlier_ratio_ = outlier_ratio; 182 | } 183 | 184 | inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) { 185 | search_method = method; 186 | } 187 | 188 | /** \brief Get the registration alignment probability. 189 | * \return transformation probability 190 | */ 191 | inline double 192 | getTransformationProbability() const 193 | { 194 | return (trans_probability_); 195 | } 196 | 197 | /** \brief Get the number of iterations required to calculate alignment. 198 | * \return final number of iterations 199 | */ 200 | inline int 201 | getFinalNumIteration() const 202 | { 203 | return (nr_iterations_); 204 | } 205 | 206 | /** \brief Convert 6 element transformation vector to affine transformation. 207 | * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 208 | * \param[out] trans affine transform corresponding to given transfomation vector 209 | */ 210 | static void 211 | convertTransform(const Eigen::Matrix &x, Eigen::Affine3f &trans) 212 | { 213 | trans = Eigen::Translation(float(x(0)), float(x(1)), float(x(2))) * 214 | Eigen::AngleAxis(float(x(3)), Eigen::Vector3f::UnitX()) * 215 | Eigen::AngleAxis(float(x(4)), Eigen::Vector3f::UnitY()) * 216 | Eigen::AngleAxis(float(x(5)), Eigen::Vector3f::UnitZ()); 217 | } 218 | 219 | /** \brief Convert 6 element transformation vector to transformation matrix. 220 | * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 221 | * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector 222 | */ 223 | static void 224 | convertTransform(const Eigen::Matrix &x, Eigen::Matrix4f &trans) 225 | { 226 | Eigen::Affine3f _affine; 227 | convertTransform(x, _affine); 228 | trans = _affine.matrix(); 229 | } 230 | 231 | // negative log likelihood function 232 | // lower is better 233 | double calculateScore(const PointCloudSource& cloud) const; 234 | 235 | protected: 236 | 237 | using pcl::Registration::reg_name_; 238 | using pcl::Registration::getClassName; 239 | using pcl::Registration::input_; 240 | using pcl::Registration::indices_; 241 | using pcl::Registration::target_; 242 | using pcl::Registration::nr_iterations_; 243 | using pcl::Registration::max_iterations_; 244 | using pcl::Registration::previous_transformation_; 245 | using pcl::Registration::final_transformation_; 246 | using pcl::Registration::transformation_; 247 | using pcl::Registration::transformation_epsilon_; 248 | using pcl::Registration::converged_; 249 | using pcl::Registration::corr_dist_threshold_; 250 | using pcl::Registration::inlier_threshold_; 251 | 252 | using pcl::Registration::update_visualizer_; 253 | 254 | /** \brief Estimate the transformation and returns the transformed source (input) as output. 255 | * \param[out] output the resultant input transfomed point cloud dataset 256 | */ 257 | virtual void 258 | computeTransformation(PointCloudSource &output) 259 | { 260 | computeTransformation(output, Eigen::Matrix4f::Identity()); 261 | } 262 | 263 | /** \brief Estimate the transformation and returns the transformed source (input) as output. 264 | * \param[out] output the resultant input transfomed point cloud dataset 265 | * \param[in] guess the initial gross estimation of the transformation 266 | */ 267 | virtual void 268 | computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess); 269 | 270 | /** \brief Initiate covariance voxel structure. */ 271 | void inline 272 | init() 273 | { 274 | target_cells_.setLeafSize(resolution_, resolution_, resolution_); 275 | target_cells_.setInputCloud(target_); 276 | // Initiate voxel structure. 277 | target_cells_.filter(true); 278 | } 279 | 280 | /** \brief Compute derivatives of probability function w.r.t. the transformation vector. 281 | * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 282 | * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 283 | * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 284 | * \param[in] trans_cloud transformed point cloud 285 | * \param[in] p the current transform vector 286 | * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 287 | */ 288 | double 289 | computeDerivatives(Eigen::Matrix &score_gradient, 290 | Eigen::Matrix &hessian, 291 | PointCloudSource &trans_cloud, 292 | Eigen::Matrix &p, 293 | bool compute_hessian = true); 294 | 295 | /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector. 296 | * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 297 | * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 298 | * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 299 | * \param[in] x_trans transformed point minus mean of occupied covariance voxel 300 | * \param[in] c_inv covariance of occupied covariance voxel 301 | * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 302 | */ 303 | double 304 | updateDerivatives(Eigen::Matrix &score_gradient, 305 | Eigen::Matrix &hessian, 306 | const Eigen::Matrix &point_gradient_, 307 | const Eigen::Matrix &point_hessian_, 308 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, 309 | bool compute_hessian = true) const; 310 | 311 | /** \brief Precompute anglular components of derivatives. 312 | * \note Equation 6.19 and 6.21 [Magnusson 2009]. 313 | * \param[in] p the current transform vector 314 | * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 315 | */ 316 | void 317 | computeAngleDerivatives(Eigen::Matrix &p, bool compute_hessian = true); 318 | 319 | /** \brief Compute point derivatives. 320 | * \note Equation 6.18-21 [Magnusson 2009]. 321 | * \param[in] x point from the input cloud 322 | * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 323 | */ 324 | void 325 | computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian = true) const; 326 | 327 | void 328 | computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian = true) const; 329 | 330 | /** \brief Compute hessian of probability function w.r.t. the transformation vector. 331 | * \note Equation 6.13 [Magnusson 2009]. 332 | * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 333 | * \param[in] trans_cloud transformed point cloud 334 | * \param[in] p the current transform vector 335 | */ 336 | void 337 | computeHessian(Eigen::Matrix &hessian, 338 | PointCloudSource &trans_cloud, 339 | Eigen::Matrix &p); 340 | 341 | /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector. 342 | * \note Equation 6.13 [Magnusson 2009]. 343 | * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 344 | * \param[in] x_trans transformed point minus mean of occupied covariance voxel 345 | * \param[in] c_inv covariance of occupied covariance voxel 346 | */ 347 | void 348 | updateHessian(Eigen::Matrix &hessian, 349 | const Eigen::Matrix &point_gradient_, 350 | const Eigen::Matrix &point_hessian_, 351 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const; 352 | 353 | /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method. 354 | * \note Search Algorithm [More, Thuente 1994] 355 | * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009] 356 | * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009] 357 | * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] 358 | * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994) 359 | * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994) 360 | * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009] 361 | * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009] 362 | * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] 363 | * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009] 364 | * \return final step length 365 | */ 366 | double 367 | computeStepLengthMT(const Eigen::Matrix &x, 368 | Eigen::Matrix &step_dir, 369 | double step_init, 370 | double step_max, double step_min, 371 | double &score, 372 | Eigen::Matrix &score_gradient, 373 | Eigen::Matrix &hessian, 374 | PointCloudSource &trans_cloud); 375 | 376 | /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994) 377 | * \note Updating Algorithm until some value satifies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 378 | * and Modified Updating Algorithm from then on [More, Thuente 1994]. 379 | * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 380 | * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm 381 | * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm 382 | * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 383 | * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm 384 | * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm 385 | * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 386 | * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm 387 | * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm 388 | * \return if interval converges 389 | */ 390 | bool 391 | updateIntervalMT(double &a_l, double &f_l, double &g_l, 392 | double &a_u, double &f_u, double &g_u, 393 | double a_t, double f_t, double g_t); 394 | 395 | /** \brief Select new trial value for More-Thuente method. 396 | * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ 397 | * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 398 | * then \f$ \phi(\alpha_k) \f$ is used from then on. 399 | * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100). 400 | * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 401 | * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) 402 | * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) 403 | * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 404 | * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) 405 | * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) 406 | * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 407 | * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994) 408 | * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994) 409 | * \return new trial value 410 | */ 411 | double 412 | trialValueSelectionMT(double a_l, double f_l, double g_l, 413 | double a_u, double f_u, double g_u, 414 | double a_t, double f_t, double g_t); 415 | 416 | /** \brief Auxilary function used to determin endpoints of More-Thuente interval. 417 | * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) 418 | * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) 419 | * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) 420 | * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) 421 | * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 422 | * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 423 | * \return sufficent decrease value 424 | */ 425 | inline double 426 | auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4) 427 | { 428 | return (f_a - f_0 - mu * g_0 * a); 429 | } 430 | 431 | /** \brief Auxilary function derivative used to determin endpoints of More-Thuente interval. 432 | * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) 433 | * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) 434 | * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 435 | * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 436 | * \return sufficent decrease derivative 437 | */ 438 | inline double 439 | auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) 440 | { 441 | return (g_a - mu * g_0); 442 | } 443 | 444 | /** \brief The voxel grid generated from target cloud containing point means and covariances. */ 445 | TargetGrid target_cells_; 446 | 447 | //double fitness_epsilon_; 448 | 449 | /** \brief The side length of voxels. */ 450 | float resolution_; 451 | 452 | /** \brief The maximum step length. */ 453 | double step_size_; 454 | 455 | /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */ 456 | double outlier_ratio_; 457 | 458 | /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */ 459 | double gauss_d1_, gauss_d2_, gauss_d3_; 460 | 461 | /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */ 462 | double trans_probability_; 463 | 464 | /** \brief Precomputed Angular Gradient 465 | * 466 | * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. 467 | */ 468 | Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_; 469 | 470 | Eigen::Matrix j_ang; 471 | 472 | /** \brief Precomputed Angular Hessian 473 | * 474 | * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009]. 475 | */ 476 | Eigen::Vector3d h_ang_a2_, h_ang_a3_, 477 | h_ang_b2_, h_ang_b3_, 478 | h_ang_c2_, h_ang_c3_, 479 | h_ang_d1_, h_ang_d2_, h_ang_d3_, 480 | h_ang_e1_, h_ang_e2_, h_ang_e3_, 481 | h_ang_f1_, h_ang_f2_, h_ang_f3_; 482 | 483 | Eigen::Matrix h_ang; 484 | 485 | /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */ 486 | // Eigen::Matrix point_gradient_; 487 | 488 | /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */ 489 | // Eigen::Matrix point_hessian_; 490 | 491 | int num_threads_; 492 | 493 | public: 494 | NeighborSearchMethod search_method; 495 | 496 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 497 | }; 498 | 499 | } 500 | 501 | #endif // PCL_REGISTRATION_NDT_H_ 502 | -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/ndt_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | #include "ndt_omp.h" 2 | /* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Point Cloud Library (PCL) - www.pointclouds.org 6 | * Copyright (c) 2010-2011, Willow Garage, Inc. 7 | * Copyright (c) 2012-, Open Perception, Inc. 8 | * 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions 13 | * are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * * Redistributions in binary form must reproduce the above 18 | * copyright notice, this list of conditions and the following 19 | * disclaimer in the documentation and/or other materials provided 20 | * with the distribution. 21 | * * Neither the name of the copyright holder(s) nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 26 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 27 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 28 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 29 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 30 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 31 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 32 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * $Id$ 39 | * 40 | */ 41 | 42 | #ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_ 43 | #define PCL_REGISTRATION_NDT_OMP_IMPL_H_ 44 | 45 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 46 | template 47 | pclomp::NormalDistributionsTransform::NormalDistributionsTransform () 48 | : target_cells_ () 49 | , resolution_ (1.0f) 50 | , step_size_ (0.1) 51 | , outlier_ratio_ (0.55) 52 | , gauss_d1_ () 53 | , gauss_d2_ () 54 | , gauss_d3_ () 55 | , trans_probability_ () 56 | , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ () 57 | , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ () 58 | , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ () 59 | { 60 | reg_name_ = "NormalDistributionsTransform"; 61 | 62 | double gauss_c1, gauss_c2; 63 | 64 | // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] 65 | gauss_c1 = 10.0 * (1 - outlier_ratio_); 66 | gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 67 | gauss_d3_ = -log (gauss_c2); 68 | gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_; 69 | gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_); 70 | 71 | transformation_epsilon_ = 0.1; 72 | max_iterations_ = 35; 73 | 74 | search_method = DIRECT7; 75 | num_threads_ = omp_get_max_threads(); 76 | } 77 | 78 | 79 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 80 | template void 81 | pclomp::NormalDistributionsTransform::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) 82 | { 83 | nr_iterations_ = 0; 84 | converged_ = false; 85 | 86 | double gauss_c1, gauss_c2; 87 | 88 | // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] 89 | gauss_c1 = 10 * (1 - outlier_ratio_); 90 | gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 91 | gauss_d3_ = -log (gauss_c2); 92 | gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_; 93 | gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_); 94 | 95 | if (guess != Eigen::Matrix4f::Identity ()) 96 | { 97 | // Initialise final transformation to the guessed one 98 | final_transformation_ = guess; 99 | // Apply guessed transformation prior to search for neighbours 100 | transformPointCloud (output, output, guess); 101 | } 102 | 103 | Eigen::Transform eig_transformation; 104 | eig_transformation.matrix () = final_transformation_; 105 | 106 | // Convert initial guess matrix to 6 element transformation vector 107 | Eigen::Matrix p, delta_p, score_gradient; 108 | Eigen::Vector3f init_translation = eig_transformation.translation (); 109 | Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2); 110 | p << init_translation (0), init_translation (1), init_translation (2), 111 | init_rotation (0), init_rotation (1), init_rotation (2); 112 | 113 | Eigen::Matrix hessian; 114 | 115 | double score = 0; 116 | double delta_p_norm; 117 | 118 | // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination. 119 | score = computeDerivatives (score_gradient, hessian, output, p); 120 | 121 | while (!converged_) 122 | { 123 | // Store previous transformation 124 | previous_transformation_ = transformation_; 125 | 126 | // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] 127 | Eigen::JacobiSVD > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); 128 | // Negative for maximization as opposed to minimization 129 | delta_p = sv.solve (-score_gradient); 130 | 131 | //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994] 132 | delta_p_norm = delta_p.norm (); 133 | 134 | if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) 135 | { 136 | trans_probability_ = score / static_cast (input_->points.size ()); 137 | converged_ = delta_p_norm == delta_p_norm; 138 | return; 139 | } 140 | 141 | delta_p.normalize (); 142 | delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output); 143 | delta_p *= delta_p_norm; 144 | 145 | 146 | transformation_ = (Eigen::Translation (static_cast (delta_p (0)), static_cast (delta_p (1)), static_cast (delta_p (2))) * 147 | Eigen::AngleAxis (static_cast (delta_p (3)), Eigen::Vector3f::UnitX ()) * 148 | Eigen::AngleAxis (static_cast (delta_p (4)), Eigen::Vector3f::UnitY ()) * 149 | Eigen::AngleAxis (static_cast (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix (); 150 | 151 | 152 | p = p + delta_p; 153 | 154 | // Update Visualizer (untested) 155 | if (update_visualizer_ != 0) 156 | update_visualizer_ (output, std::vector(), *target_, std::vector() ); 157 | 158 | if (nr_iterations_ > max_iterations_ || 159 | (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_))) 160 | { 161 | converged_ = true; 162 | } 163 | 164 | nr_iterations_++; 165 | 166 | } 167 | 168 | // Store transformation probability. The realtive differences within each scan registration are accurate 169 | // but the normalization constants need to be modified for it to be globally accurate 170 | trans_probability_ = score / static_cast (input_->points.size ()); 171 | } 172 | 173 | #ifndef _OPENMP 174 | int omp_get_max_threads() { return 1; } 175 | int omp_get_thread_num() { return 0; } 176 | #endif 177 | 178 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 179 | template double 180 | pclomp::NormalDistributionsTransform::computeDerivatives(Eigen::Matrix &score_gradient, 181 | Eigen::Matrix &hessian, 182 | PointCloudSource &trans_cloud, 183 | Eigen::Matrix &p, 184 | bool compute_hessian) 185 | { 186 | score_gradient.setZero(); 187 | hessian.setZero(); 188 | double score = 0; 189 | 190 | std::vector scores(input_->points.size()); 191 | std::vector, Eigen::aligned_allocator>> score_gradients(input_->points.size()); 192 | std::vector, Eigen::aligned_allocator>> hessians(input_->points.size()); 193 | for (int i = 0; i < input_->points.size(); i++) { 194 | scores[i] = 0; 195 | score_gradients[i].setZero(); 196 | hessians[i].setZero(); 197 | } 198 | 199 | // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] 200 | computeAngleDerivatives(p); 201 | 202 | std::vector> neighborhoods(num_threads_); 203 | std::vector> distancess(num_threads_); 204 | 205 | // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 206 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 207 | for (int idx = 0; idx < input_->points.size(); idx++) 208 | { 209 | int thread_n = omp_get_thread_num(); 210 | 211 | // Original Point and Transformed Point 212 | PointSource x_pt, x_trans_pt; 213 | // Original Point and Transformed Point (for math) 214 | Eigen::Vector3d x, x_trans; 215 | // Occupied Voxel 216 | TargetGridLeafConstPtr cell; 217 | // Inverse Covariance of Occupied Voxel 218 | Eigen::Matrix3d c_inv; 219 | 220 | // Initialize Point Gradient and Hessian 221 | Eigen::Matrix point_gradient_; 222 | Eigen::Matrix point_hessian_; 223 | point_gradient_.setZero(); 224 | point_gradient_.block<3, 3>(0, 0).setIdentity(); 225 | point_hessian_.setZero(); 226 | 227 | x_trans_pt = trans_cloud.points[idx]; 228 | 229 | auto& neighborhood = neighborhoods[thread_n]; 230 | auto& distances = distancess[thread_n]; 231 | 232 | // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. 233 | switch (search_method) { 234 | case KDTREE: 235 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 236 | break; 237 | case DIRECT26: 238 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 239 | break; 240 | default: 241 | case DIRECT7: 242 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 243 | break; 244 | case DIRECT1: 245 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 246 | break; 247 | } 248 | 249 | double score_pt = 0; 250 | Eigen::Matrix score_gradient_pt = Eigen::Matrix::Zero(); 251 | Eigen::Matrix hessian_pt = Eigen::Matrix::Zero(); 252 | 253 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) 254 | { 255 | cell = *neighborhood_it; 256 | x_pt = input_->points[idx]; 257 | x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z); 258 | 259 | x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 260 | 261 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 262 | x_trans -= cell->getMean(); 263 | // Uses precomputed covariance for speed. 264 | c_inv = cell->getInverseCov(); 265 | 266 | // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 267 | computePointDerivatives(x, point_gradient_, point_hessian_); 268 | // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 269 | score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian); 270 | } 271 | 272 | scores[idx] = score_pt; 273 | score_gradients[idx].noalias() = score_gradient_pt; 274 | hessians[idx].noalias() = hessian_pt; 275 | } 276 | 277 | // Ensure that the result is invariant against the summing up order 278 | for (int i = 0; i < input_->points.size(); i++) { 279 | score += scores[i]; 280 | score_gradient += score_gradients[i]; 281 | hessian += hessians[i]; 282 | } 283 | 284 | return (score); 285 | } 286 | 287 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 288 | template void 289 | pclomp::NormalDistributionsTransform::computeAngleDerivatives(Eigen::Matrix &p, bool compute_hessian) 290 | { 291 | // Simplified math for near 0 angles 292 | double cx, cy, cz, sx, sy, sz; 293 | if (fabs(p(3)) < 10e-5) 294 | { 295 | //p(3) = 0; 296 | cx = 1.0; 297 | sx = 0.0; 298 | } 299 | else 300 | { 301 | cx = cos(p(3)); 302 | sx = sin(p(3)); 303 | } 304 | if (fabs(p(4)) < 10e-5) 305 | { 306 | //p(4) = 0; 307 | cy = 1.0; 308 | sy = 0.0; 309 | } 310 | else 311 | { 312 | cy = cos(p(4)); 313 | sy = sin(p(4)); 314 | } 315 | 316 | if (fabs(p(5)) < 10e-5) 317 | { 318 | //p(5) = 0; 319 | cz = 1.0; 320 | sz = 0.0; 321 | } 322 | else 323 | { 324 | cz = cos(p(5)); 325 | sz = sin(p(5)); 326 | } 327 | 328 | // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009] 329 | j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy); 330 | j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy); 331 | j_ang_c_ << (-sy * cz), sy * sz, cy; 332 | j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy; 333 | j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy); 334 | j_ang_f_ << (-cy * sz), (-cy * cz), 0; 335 | j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0; 336 | j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; 337 | 338 | j_ang.setZero(); 339 | j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f); 340 | j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f); 341 | j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f); 342 | j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f); 343 | j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f); 344 | j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f); 345 | j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f); 346 | j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f); 347 | 348 | if (compute_hessian) 349 | { 350 | // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009] 351 | h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy; 352 | h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy); 353 | 354 | h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy); 355 | h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy); 356 | 357 | h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0; 358 | h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0; 359 | 360 | h_ang_d1_ << (-cy * cz), (cy * sz), (sy); 361 | h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy); 362 | h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy); 363 | 364 | h_ang_e1_ << (sy * sz), (sy * cz), 0; 365 | h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0; 366 | h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0; 367 | 368 | h_ang_f1_ << (-cy * cz), (cy * sz), 0; 369 | h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0; 370 | h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; 371 | 372 | h_ang.setZero(); 373 | h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2 374 | h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3 375 | 376 | h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2 377 | h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3 378 | 379 | h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2 380 | h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3 381 | 382 | h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f); // d1 383 | h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2 384 | h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3 385 | 386 | h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f); // e1 387 | h_ang.row(10).noalias() = Eigen::Vector4f ((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2 388 | h_ang.row(11).noalias() = Eigen::Vector4f ((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3 389 | 390 | h_ang.row(12).noalias() = Eigen::Vector4f ((-cy * cz), (cy * sz), 0, 0.0f); // f1 391 | h_ang.row(13).noalias() = Eigen::Vector4f ((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2 392 | h_ang.row(14).noalias() = Eigen::Vector4f ((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3 393 | } 394 | } 395 | 396 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 397 | template void 398 | pclomp::NormalDistributionsTransform::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian) const 399 | { 400 | Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f); 401 | 402 | // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. 403 | // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] 404 | Eigen::Matrix x_j_ang = j_ang * x4; 405 | 406 | point_gradient_(1, 3) = x_j_ang[0]; 407 | point_gradient_(2, 3) = x_j_ang[1]; 408 | point_gradient_(0, 4) = x_j_ang[2]; 409 | point_gradient_(1, 4) = x_j_ang[3]; 410 | point_gradient_(2, 4) = x_j_ang[4]; 411 | point_gradient_(0, 5) = x_j_ang[5]; 412 | point_gradient_(1, 5) = x_j_ang[6]; 413 | point_gradient_(2, 5) = x_j_ang[7]; 414 | 415 | if (compute_hessian) 416 | { 417 | Eigen::Matrix x_h_ang = h_ang * x4; 418 | 419 | // Vectors from Equation 6.21 [Magnusson 2009] 420 | Eigen::Vector4f a (0, x_h_ang[0], x_h_ang[1], 0.0f); 421 | Eigen::Vector4f b (0, x_h_ang[2], x_h_ang[3], 0.0f); 422 | Eigen::Vector4f c (0, x_h_ang[4], x_h_ang[5], 0.0f); 423 | Eigen::Vector4f d (x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f); 424 | Eigen::Vector4f e (x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f); 425 | Eigen::Vector4f f (x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f); 426 | 427 | // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. 428 | // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] 429 | point_hessian_.block<4, 1>((9/3)*4, 3) = a; 430 | point_hessian_.block<4, 1>((12/3)*4, 3) = b; 431 | point_hessian_.block<4, 1>((15/3)*4, 3) = c; 432 | point_hessian_.block<4, 1>((9/3)*4, 4) = b; 433 | point_hessian_.block<4, 1>((12/3)*4, 4) = d; 434 | point_hessian_.block<4, 1>((15/3)*4, 4) = e; 435 | point_hessian_.block<4, 1>((9/3)*4, 5) = c; 436 | point_hessian_.block<4, 1>((12/3)*4, 5) = e; 437 | point_hessian_.block<4, 1>((15/3)*4, 5) = f; 438 | } 439 | } 440 | 441 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 442 | template void 443 | pclomp::NormalDistributionsTransform::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix& point_gradient_, Eigen::Matrix& point_hessian_, bool compute_hessian) const 444 | { 445 | // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. 446 | // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] 447 | point_gradient_(1, 3) = x.dot(j_ang_a_); 448 | point_gradient_(2, 3) = x.dot(j_ang_b_); 449 | point_gradient_(0, 4) = x.dot(j_ang_c_); 450 | point_gradient_(1, 4) = x.dot(j_ang_d_); 451 | point_gradient_(2, 4) = x.dot(j_ang_e_); 452 | point_gradient_(0, 5) = x.dot(j_ang_f_); 453 | point_gradient_(1, 5) = x.dot(j_ang_g_); 454 | point_gradient_(2, 5) = x.dot(j_ang_h_); 455 | 456 | if (compute_hessian) 457 | { 458 | // Vectors from Equation 6.21 [Magnusson 2009] 459 | Eigen::Vector3d a, b, c, d, e, f; 460 | 461 | a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_); 462 | b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_); 463 | c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_); 464 | d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_); 465 | e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_); 466 | f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_); 467 | 468 | // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. 469 | // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] 470 | point_hessian_.block<3, 1>(9, 3) = a; 471 | point_hessian_.block<3, 1>(12, 3) = b; 472 | point_hessian_.block<3, 1>(15, 3) = c; 473 | point_hessian_.block<3, 1>(9, 4) = b; 474 | point_hessian_.block<3, 1>(12, 4) = d; 475 | point_hessian_.block<3, 1>(15, 4) = e; 476 | point_hessian_.block<3, 1>(9, 5) = c; 477 | point_hessian_.block<3, 1>(12, 5) = e; 478 | point_hessian_.block<3, 1>(15, 5) = f; 479 | } 480 | } 481 | 482 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 483 | template double 484 | pclomp::NormalDistributionsTransform::updateDerivatives(Eigen::Matrix &score_gradient, 485 | Eigen::Matrix &hessian, 486 | const Eigen::Matrix &point_gradient4, 487 | const Eigen::Matrix &point_hessian_, 488 | const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, 489 | bool compute_hessian) const 490 | { 491 | Eigen::Matrix x_trans4( x_trans[0], x_trans[1], x_trans[2], 0.0f ); 492 | Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero(); 493 | c_inv4.topLeftCorner(3, 3) = c_inv.cast(); 494 | 495 | float gauss_d2 = gauss_d2_; 496 | 497 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 498 | float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f); 499 | // Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009] 500 | float score_inc = -gauss_d1_ * e_x_cov_x; 501 | 502 | e_x_cov_x = gauss_d2 * e_x_cov_x; 503 | 504 | // Error checking for invalid values. 505 | if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 506 | return (0); 507 | 508 | // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 509 | e_x_cov_x *= gauss_d1_; 510 | 511 | Eigen::Matrix c_inv4_x_point_gradient4 = c_inv4 * point_gradient4; 512 | Eigen::Matrix x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4; 513 | 514 | score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast(); 515 | 516 | if (compute_hessian) { 517 | Eigen::Matrix x_trans4_x_c_inv4 = x_trans4 * c_inv4; 518 | Eigen::Matrix point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4; 519 | Eigen::Matrix x_trans4_dot_c_inv4_x_ext_point_hessian_4ij; 520 | 521 | for (int i = 0; i < 6; i++) { 522 | // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 523 | // Update gradient, Equation 6.12 [Magnusson 2009] 524 | x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0); 525 | 526 | for (int j = 0; j < hessian.cols(); j++) { 527 | // Update hessian, Equation 6.13 [Magnusson 2009] 528 | hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) + 529 | x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) + 530 | point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i)); 531 | } 532 | } 533 | } 534 | 535 | return (score_inc); 536 | } 537 | 538 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 539 | template void 540 | pclomp::NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, 541 | PointCloudSource &trans_cloud, Eigen::Matrix &) 542 | { 543 | // Original Point and Transformed Point 544 | PointSource x_pt, x_trans_pt; 545 | // Original Point and Transformed Point (for math) 546 | Eigen::Vector3d x, x_trans; 547 | // Occupied Voxel 548 | TargetGridLeafConstPtr cell; 549 | // Inverse Covariance of Occupied Voxel 550 | Eigen::Matrix3d c_inv; 551 | 552 | // Initialize Point Gradient and Hessian 553 | Eigen::Matrix point_gradient_; 554 | Eigen::Matrix point_hessian_; 555 | point_gradient_.setZero(); 556 | point_gradient_.block<3, 3>(0, 0).setIdentity(); 557 | point_hessian_.setZero(); 558 | 559 | hessian.setZero (); 560 | 561 | // Precompute Angular Derivatives unessisary because only used after regular derivative calculation 562 | 563 | // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 564 | for (size_t idx = 0; idx < input_->points.size (); idx++) 565 | { 566 | x_trans_pt = trans_cloud.points[idx]; 567 | 568 | // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. 569 | std::vector neighborhood; 570 | std::vector distances; 571 | switch (search_method) { 572 | case KDTREE: 573 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 574 | break; 575 | case DIRECT26: 576 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 577 | break; 578 | default: 579 | case DIRECT7: 580 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 581 | break; 582 | case DIRECT1: 583 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 584 | break; 585 | } 586 | 587 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) 588 | { 589 | cell = *neighborhood_it; 590 | 591 | { 592 | x_pt = input_->points[idx]; 593 | x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); 594 | 595 | x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 596 | 597 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 598 | x_trans -= cell->getMean (); 599 | // Uses precomputed covariance for speed. 600 | c_inv = cell->getInverseCov (); 601 | 602 | // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 603 | computePointDerivatives (x, point_gradient_, point_hessian_); 604 | // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 605 | updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv); 606 | } 607 | } 608 | } 609 | } 610 | 611 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 612 | template void 613 | pclomp::NormalDistributionsTransform::updateHessian (Eigen::Matrix &hessian, 614 | const Eigen::Matrix &point_gradient_, 615 | const Eigen::Matrix &point_hessian_, 616 | const Eigen::Vector3d &x_trans, 617 | const Eigen::Matrix3d &c_inv) const 618 | { 619 | Eigen::Vector3d cov_dxd_pi; 620 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 621 | double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); 622 | 623 | // Error checking for invalid values. 624 | if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 625 | return; 626 | 627 | // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 628 | e_x_cov_x *= gauss_d1_; 629 | 630 | for (int i = 0; i < 6; i++) 631 | { 632 | // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 633 | cov_dxd_pi = c_inv * point_gradient_.col (i); 634 | 635 | for (int j = 0; j < hessian.cols (); j++) 636 | { 637 | // Update hessian, Equation 6.13 [Magnusson 2009] 638 | hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + 639 | x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + 640 | point_gradient_.col (j).dot (cov_dxd_pi) ); 641 | } 642 | } 643 | 644 | } 645 | 646 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 647 | template bool 648 | pclomp::NormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, 649 | double &a_u, double &f_u, double &g_u, 650 | double a_t, double f_t, double g_t) 651 | { 652 | // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] 653 | if (f_t > f_l) 654 | { 655 | a_u = a_t; 656 | f_u = f_t; 657 | g_u = g_t; 658 | return (false); 659 | } 660 | // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] 661 | else 662 | if (g_t * (a_l - a_t) > 0) 663 | { 664 | a_l = a_t; 665 | f_l = f_t; 666 | g_l = g_t; 667 | return (false); 668 | } 669 | // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] 670 | else 671 | if (g_t * (a_l - a_t) < 0) 672 | { 673 | a_u = a_l; 674 | f_u = f_l; 675 | g_u = g_l; 676 | 677 | a_l = a_t; 678 | f_l = f_t; 679 | g_l = g_t; 680 | return (false); 681 | } 682 | // Interval Converged 683 | else 684 | return (true); 685 | } 686 | 687 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 688 | template double 689 | pclomp::NormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, 690 | double a_u, double f_u, double g_u, 691 | double a_t, double f_t, double g_t) 692 | { 693 | // Case 1 in Trial Value Selection [More, Thuente 1994] 694 | if (f_t > f_l) 695 | { 696 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 697 | // Equation 2.4.52 [Sun, Yuan 2006] 698 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 699 | double w = std::sqrt (z * z - g_t * g_l); 700 | // Equation 2.4.56 [Sun, Yuan 2006] 701 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 702 | 703 | // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l 704 | // Equation 2.4.2 [Sun, Yuan 2006] 705 | double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); 706 | 707 | if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l)) 708 | return (a_c); 709 | else 710 | return (0.5 * (a_q + a_c)); 711 | } 712 | // Case 2 in Trial Value Selection [More, Thuente 1994] 713 | else 714 | if (g_t * g_l < 0) 715 | { 716 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 717 | // Equation 2.4.52 [Sun, Yuan 2006] 718 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 719 | double w = std::sqrt (z * z - g_t * g_l); 720 | // Equation 2.4.56 [Sun, Yuan 2006] 721 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 722 | 723 | // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t 724 | // Equation 2.4.5 [Sun, Yuan 2006] 725 | double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 726 | 727 | if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t)) 728 | return (a_c); 729 | else 730 | return (a_s); 731 | } 732 | // Case 3 in Trial Value Selection [More, Thuente 1994] 733 | else 734 | if (std::fabs (g_t) <= std::fabs (g_l)) 735 | { 736 | // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 737 | // Equation 2.4.52 [Sun, Yuan 2006] 738 | double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 739 | double w = std::sqrt (z * z - g_t * g_l); 740 | double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 741 | 742 | // Calculate the minimizer of the quadratic that interpolates g_l and g_t 743 | // Equation 2.4.5 [Sun, Yuan 2006] 744 | double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 745 | 746 | double a_t_next; 747 | 748 | if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t)) 749 | a_t_next = a_c; 750 | else 751 | a_t_next = a_s; 752 | 753 | if (a_t > a_l) 754 | return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next)); 755 | else 756 | return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next)); 757 | } 758 | // Case 4 in Trial Value Selection [More, Thuente 1994] 759 | else 760 | { 761 | // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t 762 | // Equation 2.4.52 [Sun, Yuan 2006] 763 | double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; 764 | double w = std::sqrt (z * z - g_t * g_u); 765 | // Equation 2.4.56 [Sun, Yuan 2006] 766 | return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); 767 | } 768 | } 769 | 770 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 771 | template double 772 | pclomp::NormalDistributionsTransform::computeStepLengthMT (const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, 773 | double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, 774 | PointCloudSource &trans_cloud) 775 | { 776 | // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] 777 | double phi_0 = -score; 778 | // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] 779 | double d_phi_0 = -(score_gradient.dot (step_dir)); 780 | 781 | Eigen::Matrix x_t; 782 | 783 | if (d_phi_0 >= 0) 784 | { 785 | // Not a decent direction 786 | if (d_phi_0 == 0) 787 | return 0; 788 | else 789 | { 790 | // Reverse step direction and calculate optimal step. 791 | d_phi_0 *= -1; 792 | step_dir *= -1; 793 | 794 | } 795 | } 796 | 797 | // The Search Algorithm for T(mu) [More, Thuente 1994] 798 | 799 | int max_step_iterations = 10; 800 | int step_iterations = 0; 801 | 802 | // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994] 803 | double mu = 1.e-4; 804 | // Curvature condition constant, Equation 1.2 [More, Thuete 1994] 805 | double nu = 0.9; 806 | 807 | // Initial endpoints of Interval I, 808 | double a_l = 0, a_u = 0; 809 | 810 | // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994] 811 | double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu); 812 | double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 813 | 814 | double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu); 815 | double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 816 | 817 | // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max 818 | bool interval_converged = (step_max - step_min) < 0, open_interval = true; 819 | 820 | double a_t = step_init; 821 | a_t = std::min (a_t, step_max); 822 | a_t = std::max (a_t, step_min); 823 | 824 | x_t = x + step_dir * a_t; 825 | 826 | final_transformation_ = (Eigen::Translation(static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * 827 | Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * 828 | Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * 829 | Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 830 | 831 | // New transformed point cloud 832 | transformPointCloud (*input_, trans_cloud, final_transformation_); 833 | 834 | // Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step calculations use the 835 | // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time. 836 | score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true); 837 | 838 | // Calculate phi(alpha_t) 839 | double phi_t = -score; 840 | // Calculate phi'(alpha_t) 841 | double d_phi_t = -(score_gradient.dot (step_dir)); 842 | 843 | // Calculate psi(alpha_t) 844 | double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 845 | // Calculate psi'(alpha_t) 846 | double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 847 | 848 | // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] 849 | while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) 850 | { 851 | // Use auxilary function if interval I is not closed 852 | if (open_interval) 853 | { 854 | a_t = trialValueSelectionMT (a_l, f_l, g_l, 855 | a_u, f_u, g_u, 856 | a_t, psi_t, d_psi_t); 857 | } 858 | else 859 | { 860 | a_t = trialValueSelectionMT (a_l, f_l, g_l, 861 | a_u, f_u, g_u, 862 | a_t, phi_t, d_phi_t); 863 | } 864 | 865 | a_t = std::min (a_t, step_max); 866 | a_t = std::max (a_t, step_min); 867 | 868 | x_t = x + step_dir * a_t; 869 | 870 | final_transformation_ = (Eigen::Translation (static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * 871 | Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * 872 | Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * 873 | Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 874 | 875 | // New transformed point cloud 876 | // Done on final cloud to prevent wasted computation 877 | transformPointCloud (*input_, trans_cloud, final_transformation_); 878 | 879 | // Updates score, gradient. Values stored to prevent wasted computation. 880 | score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false); 881 | 882 | // Calculate phi(alpha_t+) 883 | phi_t = -score; 884 | // Calculate phi'(alpha_t+) 885 | d_phi_t = -(score_gradient.dot (step_dir)); 886 | 887 | // Calculate psi(alpha_t+) 888 | psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 889 | // Calculate psi'(alpha_t+) 890 | d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 891 | 892 | // Check if I is now a closed interval 893 | if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) 894 | { 895 | open_interval = false; 896 | 897 | // Converts f_l and g_l from psi to phi 898 | f_l = f_l + phi_0 - mu * d_phi_0 * a_l; 899 | g_l = g_l + mu * d_phi_0; 900 | 901 | // Converts f_u and g_u from psi to phi 902 | f_u = f_u + phi_0 - mu * d_phi_0 * a_u; 903 | g_u = g_u + mu * d_phi_0; 904 | } 905 | 906 | if (open_interval) 907 | { 908 | // Update interval end points using Updating Algorithm [More, Thuente 1994] 909 | interval_converged = updateIntervalMT (a_l, f_l, g_l, 910 | a_u, f_u, g_u, 911 | a_t, psi_t, d_psi_t); 912 | } 913 | else 914 | { 915 | // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] 916 | interval_converged = updateIntervalMT (a_l, f_l, g_l, 917 | a_u, f_u, g_u, 918 | a_t, phi_t, d_phi_t); 919 | } 920 | 921 | step_iterations++; 922 | } 923 | 924 | // If inner loop was run then hessian needs to be calculated. 925 | // Hessian is unnessisary for step length determination but gradients are required 926 | // so derivative and transform data is stored for the next iteration. 927 | if (step_iterations) 928 | computeHessian (hessian, trans_cloud, x_t); 929 | 930 | return (a_t); 931 | } 932 | 933 | 934 | template 935 | double pclomp::NormalDistributionsTransform::calculateScore(const PointCloudSource & trans_cloud) const 936 | { 937 | double score = 0; 938 | 939 | for (int idx = 0; idx < trans_cloud.points.size(); idx++) 940 | { 941 | PointSource x_trans_pt = trans_cloud.points[idx]; 942 | 943 | // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. 944 | std::vector neighborhood; 945 | std::vector distances; 946 | switch (search_method) { 947 | case KDTREE: 948 | target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); 949 | break; 950 | case DIRECT26: 951 | target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); 952 | break; 953 | default: 954 | case DIRECT7: 955 | target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); 956 | break; 957 | case DIRECT1: 958 | target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); 959 | break; 960 | } 961 | 962 | for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) 963 | { 964 | TargetGridLeafConstPtr cell = *neighborhood_it; 965 | 966 | Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 967 | 968 | // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 969 | x_trans -= cell->getMean(); 970 | // Uses precomputed covariance for speed. 971 | Eigen::Matrix3d c_inv = cell->getInverseCov(); 972 | 973 | // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 974 | double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); 975 | // Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009] 976 | double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_; 977 | 978 | score += score_inc / neighborhood.size(); 979 | } 980 | } 981 | return (score) / static_cast (trans_cloud.size()); 982 | } 983 | 984 | #endif // PCL_REGISTRATION_NDT_IMPL_H_ 985 | -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/voxel_grid_covariance_omp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace pclomp 49 | { 50 | /** \brief A searchable voxel strucure containing the mean and covariance of the data. 51 | * \note For more information please see 52 | * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 53 | * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 54 | * PhD thesis, Orebro University. Orebro Studies in Technology 36 55 | * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 56 | */ 57 | template 58 | class VoxelGridCovariance : public pcl::VoxelGrid 59 | { 60 | protected: 61 | using pcl::VoxelGrid::filter_name_; 62 | using pcl::VoxelGrid::getClassName; 63 | using pcl::VoxelGrid::input_; 64 | using pcl::VoxelGrid::indices_; 65 | using pcl::VoxelGrid::filter_limit_negative_; 66 | using pcl::VoxelGrid::filter_limit_min_; 67 | using pcl::VoxelGrid::filter_limit_max_; 68 | using pcl::VoxelGrid::filter_field_name_; 69 | 70 | using pcl::VoxelGrid::downsample_all_data_; 71 | using pcl::VoxelGrid::leaf_layout_; 72 | using pcl::VoxelGrid::save_leaf_layout_; 73 | using pcl::VoxelGrid::leaf_size_; 74 | using pcl::VoxelGrid::min_b_; 75 | using pcl::VoxelGrid::max_b_; 76 | using pcl::VoxelGrid::inverse_leaf_size_; 77 | using pcl::VoxelGrid::div_b_; 78 | using pcl::VoxelGrid::divb_mul_; 79 | 80 | typedef typename pcl::traits::fieldList::type FieldList; 81 | typedef typename pcl::Filter::PointCloud PointCloud; 82 | typedef typename PointCloud::Ptr PointCloudPtr; 83 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 84 | 85 | public: 86 | 87 | typedef boost::shared_ptr< pcl::VoxelGrid > Ptr; 88 | typedef boost::shared_ptr< const pcl::VoxelGrid > ConstPtr; 89 | 90 | /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf. 91 | * Inverse covariance, eigen vectors and engen values are precomputed. */ 92 | struct Leaf 93 | { 94 | /** \brief Constructor. 95 | * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix 96 | */ 97 | Leaf () : 98 | nr_points (0), 99 | mean_ (Eigen::Vector3d::Zero ()), 100 | centroid (), 101 | cov_ (Eigen::Matrix3d::Identity ()), 102 | icov_ (Eigen::Matrix3d::Zero ()), 103 | evecs_ (Eigen::Matrix3d::Identity ()), 104 | evals_ (Eigen::Vector3d::Zero ()) 105 | { 106 | } 107 | 108 | /** \brief Get the voxel covariance. 109 | * \return covariance matrix 110 | */ 111 | Eigen::Matrix3d 112 | getCov () const 113 | { 114 | return (cov_); 115 | } 116 | 117 | /** \brief Get the inverse of the voxel covariance. 118 | * \return inverse covariance matrix 119 | */ 120 | Eigen::Matrix3d 121 | getInverseCov () const 122 | { 123 | return (icov_); 124 | } 125 | 126 | /** \brief Get the voxel centroid. 127 | * \return centroid 128 | */ 129 | Eigen::Vector3d 130 | getMean () const 131 | { 132 | return (mean_); 133 | } 134 | 135 | /** \brief Get the eigen vectors of the voxel covariance. 136 | * \note Order corresponds with \ref getEvals 137 | * \return matrix whose columns contain eigen vectors 138 | */ 139 | Eigen::Matrix3d 140 | getEvecs () const 141 | { 142 | return (evecs_); 143 | } 144 | 145 | /** \brief Get the eigen values of the voxel covariance. 146 | * \note Order corresponds with \ref getEvecs 147 | * \return vector of eigen values 148 | */ 149 | Eigen::Vector3d 150 | getEvals () const 151 | { 152 | return (evals_); 153 | } 154 | 155 | /** \brief Get the number of points contained by this voxel. 156 | * \return number of points 157 | */ 158 | int 159 | getPointCount () const 160 | { 161 | return (nr_points); 162 | } 163 | 164 | /** \brief Number of points contained by voxel */ 165 | int nr_points; 166 | 167 | /** \brief 3D voxel centroid */ 168 | Eigen::Vector3d mean_; 169 | 170 | /** \brief Nd voxel centroid 171 | * \note Differs from \ref mean_ when color data is used 172 | */ 173 | Eigen::VectorXf centroid; 174 | 175 | /** \brief Voxel covariance matrix */ 176 | Eigen::Matrix3d cov_; 177 | 178 | /** \brief Inverse of voxel covariance matrix */ 179 | Eigen::Matrix3d icov_; 180 | 181 | /** \brief Eigen vectors of voxel covariance matrix */ 182 | Eigen::Matrix3d evecs_; 183 | 184 | /** \brief Eigen values of voxel covariance matrix */ 185 | Eigen::Vector3d evals_; 186 | 187 | }; 188 | 189 | /** \brief Pointer to VoxelGridCovariance leaf structure */ 190 | typedef Leaf* LeafPtr; 191 | 192 | /** \brief Const pointer to VoxelGridCovariance leaf structure */ 193 | typedef const Leaf* LeafConstPtr; 194 | 195 | typedef std::map Map; 196 | 197 | public: 198 | 199 | /** \brief Constructor. 200 | * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. 201 | */ 202 | VoxelGridCovariance () : 203 | searchable_ (true), 204 | min_points_per_voxel_ (6), 205 | min_covar_eigvalue_mult_ (0.01), 206 | leaves_ (), 207 | voxel_centroids_ (), 208 | voxel_centroids_leaf_indices_ (), 209 | kdtree_ () 210 | { 211 | downsample_all_data_ = false; 212 | save_leaf_layout_ = false; 213 | leaf_size_.setZero (); 214 | min_b_.setZero (); 215 | max_b_.setZero (); 216 | filter_name_ = "VoxelGridCovariance"; 217 | } 218 | 219 | /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation). 220 | * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used 221 | */ 222 | inline void 223 | setMinPointPerVoxel (int min_points_per_voxel) 224 | { 225 | if(min_points_per_voxel > 2) 226 | { 227 | min_points_per_voxel_ = min_points_per_voxel; 228 | } 229 | else 230 | { 231 | PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ()); 232 | min_points_per_voxel_ = 3; 233 | } 234 | } 235 | 236 | /** \brief Get the minimum number of points required for a cell to be used. 237 | * \return the minimum number of points for required for a voxel to be used 238 | */ 239 | inline int 240 | getMinPointPerVoxel () 241 | { 242 | return min_points_per_voxel_; 243 | } 244 | 245 | /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 246 | * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues 247 | */ 248 | inline void 249 | setCovEigValueInflationRatio (double min_covar_eigvalue_mult) 250 | { 251 | min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; 252 | } 253 | 254 | /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 255 | * \return the minimum allowable ratio between eigenvalues 256 | */ 257 | inline double 258 | getCovEigValueInflationRatio () 259 | { 260 | return min_covar_eigvalue_mult_; 261 | } 262 | 263 | /** \brief Filter cloud and initializes voxel structure. 264 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 265 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 266 | */ 267 | inline void 268 | filter (PointCloud &output, bool searchable = false) 269 | { 270 | searchable_ = searchable; 271 | applyFilter (output); 272 | 273 | voxel_centroids_ = PointCloudPtr (new PointCloud (output)); 274 | 275 | if (searchable_ && voxel_centroids_->size() > 0) 276 | { 277 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 278 | kdtree_.setInputCloud (voxel_centroids_); 279 | } 280 | } 281 | 282 | /** \brief Initializes voxel structure. 283 | * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 284 | */ 285 | inline void 286 | filter (bool searchable = false) 287 | { 288 | searchable_ = searchable; 289 | voxel_centroids_ = PointCloudPtr (new PointCloud); 290 | applyFilter (*voxel_centroids_); 291 | 292 | if (searchable_ && voxel_centroids_->size() > 0) 293 | { 294 | // Initiates kdtree of the centroids of voxels containing a sufficient number of points 295 | kdtree_.setInputCloud (voxel_centroids_); 296 | } 297 | } 298 | 299 | /** \brief Get the voxel containing point p. 300 | * \param[in] index the index of the leaf structure node 301 | * \return const pointer to leaf structure 302 | */ 303 | inline LeafConstPtr 304 | getLeaf (int index) 305 | { 306 | auto leaf_iter = leaves_.find (index); 307 | if (leaf_iter != leaves_.end ()) 308 | { 309 | LeafConstPtr ret (&(leaf_iter->second)); 310 | return ret; 311 | } 312 | else 313 | return NULL; 314 | } 315 | 316 | /** \brief Get the voxel containing point p. 317 | * \param[in] p the point to get the leaf structure at 318 | * \return const pointer to leaf structure 319 | */ 320 | inline LeafConstPtr 321 | getLeaf (PointT &p) 322 | { 323 | // Generate index associated with p 324 | int ijk0 = static_cast (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]); 325 | int ijk1 = static_cast (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]); 326 | int ijk2 = static_cast (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]); 327 | 328 | // Compute the centroid leaf index 329 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 330 | 331 | // Find leaf associated with index 332 | auto leaf_iter = leaves_.find (idx); 333 | if (leaf_iter != leaves_.end ()) 334 | { 335 | // If such a leaf exists return the pointer to the leaf structure 336 | LeafConstPtr ret (&(leaf_iter->second)); 337 | return ret; 338 | } 339 | else 340 | return NULL; 341 | } 342 | 343 | /** \brief Get the voxel containing point p. 344 | * \param[in] p the point to get the leaf structure at 345 | * \return const pointer to leaf structure 346 | */ 347 | inline LeafConstPtr 348 | getLeaf (Eigen::Vector3f &p) 349 | { 350 | // Generate index associated with p 351 | int ijk0 = static_cast (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]); 352 | int ijk1 = static_cast (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]); 353 | int ijk2 = static_cast (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]); 354 | 355 | // Compute the centroid leaf index 356 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 357 | 358 | // Find leaf associated with index 359 | auto leaf_iter = leaves_.find (idx); 360 | if (leaf_iter != leaves_.end ()) 361 | { 362 | // If such a leaf exists return the pointer to the leaf structure 363 | LeafConstPtr ret (&(leaf_iter->second)); 364 | return ret; 365 | } 366 | else 367 | return NULL; 368 | 369 | } 370 | 371 | /** \brief Get the voxels surrounding point p, not including the voxel contating point p. 372 | * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). 373 | * \param[in] reference_point the point to get the leaf structure at 374 | * \param[out] neighbors 375 | * \return number of neighbors found 376 | */ 377 | int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector &neighbors) const ; 378 | int getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const ; 379 | int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const ; 380 | int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const ; 381 | 382 | /** \brief Get the leaf structure map 383 | * \return a map contataining all leaves 384 | */ 385 | inline const Map& 386 | getLeaves () 387 | { 388 | return leaves_; 389 | } 390 | 391 | /** \brief Get a pointcloud containing the voxel centroids 392 | * \note Only voxels containing a sufficient number of points are used. 393 | * \return a map contataining all leaves 394 | */ 395 | inline PointCloudPtr 396 | getCentroids () 397 | { 398 | return voxel_centroids_; 399 | } 400 | 401 | 402 | /** \brief Get a cloud to visualize each voxels normal distribution. 403 | * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel 404 | */ 405 | void 406 | getDisplayCloud (pcl::PointCloud& cell_cloud); 407 | 408 | /** \brief Search for the k-nearest occupied voxels for the given query point. 409 | * \note Only voxels containing a sufficient number of points are used. 410 | * \param[in] point the given query point 411 | * \param[in] k the number of neighbors to search for 412 | * \param[out] k_leaves the resultant leaves of the neighboring points 413 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 414 | * \return number of neighbors found 415 | */ 416 | int 417 | nearestKSearch (const PointT &point, int k, 418 | std::vector &k_leaves, std::vector &k_sqr_distances) 419 | { 420 | k_leaves.clear (); 421 | 422 | // Check if kdtree has been built 423 | if (!searchable_) 424 | { 425 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 426 | return 0; 427 | } 428 | 429 | // Find k-nearest neighbors in the occupied voxel centroid cloud 430 | std::vector k_indices; 431 | k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances); 432 | 433 | // Find leaves corresponding to neighbors 434 | k_leaves.reserve (k); 435 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 436 | { 437 | k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]); 438 | } 439 | return k; 440 | } 441 | 442 | /** \brief Search for the k-nearest occupied voxels for the given query point. 443 | * \note Only voxels containing a sufficient number of points are used. 444 | * \param[in] cloud the given query point 445 | * \param[in] index the index 446 | * \param[in] k the number of neighbors to search for 447 | * \param[out] k_leaves the resultant leaves of the neighboring points 448 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 449 | * \return number of neighbors found 450 | */ 451 | inline int 452 | nearestKSearch (const PointCloud &cloud, int index, int k, 453 | std::vector &k_leaves, std::vector &k_sqr_distances) 454 | { 455 | if (index >= static_cast (cloud.points.size ()) || index < 0) 456 | return (0); 457 | return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances)); 458 | } 459 | 460 | 461 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 462 | * \note Only voxels containing a sufficient number of points are used. 463 | * \param[in] point the given query point 464 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 465 | * \param[out] k_leaves the resultant leaves of the neighboring points 466 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 467 | * \param[in] max_nn 468 | * \return number of neighbors found 469 | */ 470 | int 471 | radiusSearch (const PointT &point, double radius, std::vector &k_leaves, 472 | std::vector &k_sqr_distances, unsigned int max_nn = 0) const 473 | { 474 | k_leaves.clear (); 475 | 476 | // Check if kdtree has been built 477 | if (!searchable_) 478 | { 479 | PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 480 | return 0; 481 | } 482 | 483 | // Find neighbors within radius in the occupied voxel centroid cloud 484 | std::vector k_indices; 485 | int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); 486 | 487 | // Find leaves corresponding to neighbors 488 | k_leaves.reserve (k); 489 | for (std::vector::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 490 | { 491 | auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]); 492 | if (leaf == leaves_.end()) { 493 | std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; 494 | std::cin.ignore(1); 495 | } 496 | k_leaves.push_back (&(leaf->second)); 497 | } 498 | return k; 499 | } 500 | 501 | /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 502 | * \note Only voxels containing a sufficient number of points are used. 503 | * \param[in] cloud the given query point 504 | * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point 505 | * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 506 | * \param[out] k_leaves the resultant leaves of the neighboring points 507 | * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 508 | * \param[in] max_nn 509 | * \return number of neighbors found 510 | */ 511 | inline int 512 | radiusSearch (const PointCloud &cloud, int index, double radius, 513 | std::vector &k_leaves, std::vector &k_sqr_distances, 514 | unsigned int max_nn = 0) const 515 | { 516 | if (index >= static_cast (cloud.points.size ()) || index < 0) 517 | return (0); 518 | return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); 519 | } 520 | 521 | protected: 522 | 523 | /** \brief Filter cloud and initializes voxel structure. 524 | * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 525 | */ 526 | void applyFilter (PointCloud &output); 527 | 528 | /** \brief Flag to determine if voxel structure is searchable. */ 529 | bool searchable_; 530 | 531 | /** \brief Minimum points contained with in a voxel to allow it to be useable. */ 532 | int min_points_per_voxel_; 533 | 534 | /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ 535 | double min_covar_eigvalue_mult_; 536 | 537 | /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ 538 | Map leaves_; 539 | 540 | /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */ 541 | PointCloudPtr voxel_centroids_; 542 | 543 | /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */ 544 | std::vector voxel_centroids_leaf_indices_; 545 | 546 | /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ 547 | pcl::KdTreeFLANN kdtree_; 548 | }; 549 | } 550 | 551 | #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_ 552 | -------------------------------------------------------------------------------- /ndt_omp/include/pclomp/voxel_grid_covariance_omp_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 39 | #define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ 40 | 41 | #include 42 | #include 43 | #include "voxel_grid_covariance_omp.h" 44 | #include 45 | #include 46 | 47 | ////////////////////////////////////////////////////////////////////////////////////////// 48 | template void 49 | pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) 50 | { 51 | voxel_centroids_leaf_indices_.clear (); 52 | 53 | // Has the input dataset been set already? 54 | if (!input_) 55 | { 56 | PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 57 | output.width = output.height = 0; 58 | output.points.clear (); 59 | return; 60 | } 61 | 62 | // Copy the header (and thus the frame_id) + allocate enough space for points 63 | output.height = 1; // downsampling breaks the organized structure 64 | output.is_dense = true; // we filter out invalid points 65 | output.points.clear (); 66 | 67 | Eigen::Vector4f min_p, max_p; 68 | // Get the minimum and maximum dimensions 69 | if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... 70 | pcl::getMinMax3D (input_, filter_field_name_, static_cast (filter_limit_min_), static_cast (filter_limit_max_), min_p, max_p, filter_limit_negative_); 71 | else 72 | pcl::getMinMax3D (*input_, min_p, max_p); 73 | 74 | // Check that the leaf size is not too small, given the size of the data 75 | int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; 76 | int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; 77 | int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; 78 | 79 | if((dx*dy*dz) > std::numeric_limits::max()) 80 | { 81 | PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); 82 | output.clear(); 83 | return; 84 | } 85 | 86 | // Compute the minimum and maximum bounding box values 87 | min_b_[0] = static_cast (floor (min_p[0] * inverse_leaf_size_[0])); 88 | max_b_[0] = static_cast (floor (max_p[0] * inverse_leaf_size_[0])); 89 | min_b_[1] = static_cast (floor (min_p[1] * inverse_leaf_size_[1])); 90 | max_b_[1] = static_cast (floor (max_p[1] * inverse_leaf_size_[1])); 91 | min_b_[2] = static_cast (floor (min_p[2] * inverse_leaf_size_[2])); 92 | max_b_[2] = static_cast (floor (max_p[2] * inverse_leaf_size_[2])); 93 | 94 | // Compute the number of divisions needed along all axis 95 | div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 96 | div_b_[3] = 0; 97 | 98 | // Clear the leaves 99 | leaves_.clear (); 100 | // leaves_.reserve(8192); 101 | 102 | // Set up the division multiplier 103 | divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 104 | 105 | int centroid_size = 4; 106 | 107 | if (downsample_all_data_) 108 | centroid_size = boost::mpl::size::value; 109 | 110 | // ---[ RGB special case 111 | std::vector fields; 112 | int rgba_index = -1; 113 | rgba_index = pcl::getFieldIndex (*input_, "rgb", fields); 114 | if (rgba_index == -1) 115 | rgba_index = pcl::getFieldIndex (*input_, "rgba", fields); 116 | if (rgba_index >= 0) 117 | { 118 | rgba_index = fields[rgba_index].offset; 119 | centroid_size += 3; 120 | } 121 | 122 | // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 123 | if (!filter_field_name_.empty ()) 124 | { 125 | // Get the distance field index 126 | std::vector fields; 127 | int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields); 128 | if (distance_idx == -1) 129 | PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 130 | 131 | // First pass: go over all points and insert them into the right leaf 132 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 133 | { 134 | if (!input_->is_dense) 135 | // Check if the point is invalid 136 | if (!pcl_isfinite (input_->points[cp].x) || 137 | !pcl_isfinite (input_->points[cp].y) || 138 | !pcl_isfinite (input_->points[cp].z)) 139 | continue; 140 | 141 | // Get the distance value 142 | const uint8_t* pt_data = reinterpret_cast (&input_->points[cp]); 143 | float distance_value = 0; 144 | memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 145 | 146 | if (filter_limit_negative_) 147 | { 148 | // Use a threshold for cutting out points which inside the interval 149 | if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 150 | continue; 151 | } 152 | else 153 | { 154 | // Use a threshold for cutting out points which are too close/far away 155 | if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 156 | continue; 157 | } 158 | 159 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 160 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 161 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 162 | 163 | // Compute the centroid leaf index 164 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 165 | 166 | Leaf& leaf = leaves_[idx]; 167 | if (leaf.nr_points == 0) 168 | { 169 | leaf.centroid.resize (centroid_size); 170 | leaf.centroid.setZero (); 171 | } 172 | 173 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 174 | // Accumulate point sum for centroid calculation 175 | leaf.mean_ += pt3d; 176 | // Accumulate x*xT for single pass covariance calculation 177 | leaf.cov_ += pt3d * pt3d.transpose (); 178 | 179 | // Do we need to process all the fields? 180 | if (!downsample_all_data_) 181 | { 182 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 183 | leaf.centroid.template head<4> () += pt; 184 | } 185 | else 186 | { 187 | // Copy all the fields 188 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 189 | // ---[ RGB special case 190 | if (rgba_index >= 0) 191 | { 192 | // fill r/g/b data 193 | int rgb; 194 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 195 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 196 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 197 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 198 | } 199 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 200 | leaf.centroid += centroid; 201 | } 202 | ++leaf.nr_points; 203 | } 204 | } 205 | // No distance filtering, process all data 206 | else 207 | { 208 | // First pass: go over all points and insert them into the right leaf 209 | for (size_t cp = 0; cp < input_->points.size (); ++cp) 210 | { 211 | if (!input_->is_dense) 212 | // Check if the point is invalid 213 | if (!pcl_isfinite (input_->points[cp].x) || 214 | !pcl_isfinite (input_->points[cp].y) || 215 | !pcl_isfinite (input_->points[cp].z)) 216 | continue; 217 | 218 | int ijk0 = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); 219 | int ijk1 = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); 220 | int ijk2 = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); 221 | 222 | // Compute the centroid leaf index 223 | int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 224 | 225 | //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast ()).matrix () - min_b_).dot (divb_mul_); 226 | Leaf& leaf = leaves_[idx]; 227 | if (leaf.nr_points == 0) 228 | { 229 | leaf.centroid.resize (centroid_size); 230 | leaf.centroid.setZero (); 231 | } 232 | 233 | Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 234 | // Accumulate point sum for centroid calculation 235 | leaf.mean_ += pt3d; 236 | // Accumulate x*xT for single pass covariance calculation 237 | leaf.cov_ += pt3d * pt3d.transpose (); 238 | 239 | // Do we need to process all the fields? 240 | if (!downsample_all_data_) 241 | { 242 | Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 243 | leaf.centroid.template head<4> () += pt; 244 | } 245 | else 246 | { 247 | // Copy all the fields 248 | Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 249 | // ---[ RGB special case 250 | if (rgba_index >= 0) 251 | { 252 | // Fill r/g/b data, assuming that the order is BGRA 253 | int rgb; 254 | memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); 255 | centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); 256 | centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); 257 | centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); 258 | } 259 | pcl::for_each_type (pcl::NdCopyPointEigenFunctor (input_->points[cp], centroid)); 260 | leaf.centroid += centroid; 261 | } 262 | ++leaf.nr_points; 263 | } 264 | } 265 | 266 | // Second pass: go over all leaves and compute centroids and covariance matrices 267 | output.points.reserve (leaves_.size ()); 268 | if (searchable_) 269 | voxel_centroids_leaf_indices_.reserve (leaves_.size ()); 270 | int cp = 0; 271 | if (save_leaf_layout_) 272 | leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1); 273 | 274 | // Eigen values and vectors calculated to prevent near singluar matrices 275 | Eigen::SelfAdjointEigenSolver eigensolver; 276 | Eigen::Matrix3d eigen_val; 277 | Eigen::Vector3d pt_sum; 278 | 279 | // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value. 280 | double min_covar_eigvalue; 281 | 282 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 283 | { 284 | 285 | // Normalize the centroid 286 | Leaf& leaf = it->second; 287 | 288 | // Normalize the centroid 289 | leaf.centroid /= static_cast (leaf.nr_points); 290 | // Point sum used for single pass covariance calculation 291 | pt_sum = leaf.mean_; 292 | // Normalize mean 293 | leaf.mean_ /= leaf.nr_points; 294 | 295 | // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds. 296 | // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution. 297 | if (leaf.nr_points >= min_points_per_voxel_) 298 | { 299 | if (save_leaf_layout_) 300 | leaf_layout_[it->first] = cp++; 301 | 302 | output.push_back (PointT ()); 303 | 304 | // Do we need to process all the fields? 305 | if (!downsample_all_data_) 306 | { 307 | output.points.back ().x = leaf.centroid[0]; 308 | output.points.back ().y = leaf.centroid[1]; 309 | output.points.back ().z = leaf.centroid[2]; 310 | } 311 | else 312 | { 313 | pcl::for_each_type (pcl::NdCopyEigenPointFunctor (leaf.centroid, output.back ())); 314 | // ---[ RGB special case 315 | if (rgba_index >= 0) 316 | { 317 | // pack r/g/b into rgb 318 | float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1]; 319 | int rgb = (static_cast (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); 320 | memcpy (reinterpret_cast (&output.points.back ()) + rgba_index, &rgb, sizeof (float)); 321 | } 322 | } 323 | 324 | // Stores the voxel indice for fast access searching 325 | if (searchable_) 326 | voxel_centroids_leaf_indices_.push_back (static_cast (it->first)); 327 | 328 | // Single pass covariance calculation 329 | leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose (); 330 | leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; 331 | 332 | //Normalize Eigen Val such that max no more than 100x min. 333 | eigensolver.compute (leaf.cov_); 334 | eigen_val = eigensolver.eigenvalues ().asDiagonal (); 335 | leaf.evecs_ = eigensolver.eigenvectors (); 336 | 337 | if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) 338 | { 339 | leaf.nr_points = -1; 340 | continue; 341 | } 342 | 343 | // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] 344 | 345 | min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2); 346 | if (eigen_val (0, 0) < min_covar_eigvalue) 347 | { 348 | eigen_val (0, 0) = min_covar_eigvalue; 349 | 350 | if (eigen_val (1, 1) < min_covar_eigvalue) 351 | { 352 | eigen_val (1, 1) = min_covar_eigvalue; 353 | } 354 | 355 | leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); 356 | } 357 | leaf.evals_ = eigen_val.diagonal (); 358 | 359 | leaf.icov_ = leaf.cov_.inverse (); 360 | if (leaf.icov_.maxCoeff () == std::numeric_limits::infinity ( ) 361 | || leaf.icov_.minCoeff () == -std::numeric_limits::infinity ( ) ) 362 | { 363 | leaf.nr_points = -1; 364 | } 365 | 366 | } 367 | } 368 | 369 | output.width = static_cast (output.points.size ()); 370 | } 371 | 372 | ////////////////////////////////////////////////////////////////////////////////////////// 373 | template int 374 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector &neighbors) const 375 | { 376 | neighbors.clear(); 377 | 378 | // Find displacement coordinates 379 | Eigen::Vector4i ijk(static_cast (floor(reference_point.x / leaf_size_[0])), 380 | static_cast (floor(reference_point.y / leaf_size_[1])), 381 | static_cast (floor(reference_point.z / leaf_size_[2])), 0); 382 | Eigen::Array4i diff2min = min_b_ - ijk; 383 | Eigen::Array4i diff2max = max_b_ - ijk; 384 | neighbors.reserve(relative_coordinates.cols()); 385 | 386 | // Check each neighbor to see if it is occupied and contains sufficient points 387 | // Slower than radius search because needs to check 26 indices 388 | for (int ni = 0; ni < relative_coordinates.cols(); ni++) 389 | { 390 | Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 391 | // Checking if the specified cell is in the grid 392 | if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 393 | { 394 | auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_))); 395 | if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_) 396 | { 397 | LeafConstPtr leaf = &(leaf_iter->second); 398 | neighbors.push_back(leaf); 399 | } 400 | } 401 | } 402 | 403 | return (static_cast (neighbors.size())); 404 | } 405 | 406 | ////////////////////////////////////////////////////////////////////////////////////////// 407 | template int 408 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const PointT& reference_point, std::vector &neighbors) const 409 | { 410 | neighbors.clear(); 411 | 412 | // Find displacement coordinates 413 | Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices(); 414 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 415 | } 416 | 417 | ////////////////////////////////////////////////////////////////////////////////////////// 418 | template int 419 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector &neighbors) const 420 | { 421 | neighbors.clear(); 422 | 423 | Eigen::MatrixXi relative_coordinates(3, 7); 424 | relative_coordinates.setZero(); 425 | relative_coordinates(0, 1) = 1; 426 | relative_coordinates(0, 2) = -1; 427 | relative_coordinates(1, 3) = 1; 428 | relative_coordinates(1, 4) = -1; 429 | relative_coordinates(2, 5) = 1; 430 | relative_coordinates(2, 6) = -1; 431 | 432 | return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); 433 | } 434 | 435 | 436 | ////////////////////////////////////////////////////////////////////////////////////////// 437 | template int 438 | pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector &neighbors) const 439 | { 440 | neighbors.clear(); 441 | return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors); 442 | } 443 | 444 | 445 | 446 | ////////////////////////////////////////////////////////////////////////////////////////// 447 | template void 448 | pclomp::VoxelGridCovariance::getDisplayCloud (pcl::PointCloud& cell_cloud) 449 | { 450 | cell_cloud.clear (); 451 | 452 | int pnt_per_cell = 1000; 453 | boost::mt19937 rng; 454 | boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ()); 455 | boost::variate_generator > var_nor (rng, nd); 456 | 457 | Eigen::LLT llt_of_cov; 458 | Eigen::Matrix3d cholesky_decomp; 459 | Eigen::Vector3d cell_mean; 460 | Eigen::Vector3d rand_point; 461 | Eigen::Vector3d dist_point; 462 | 463 | // Generate points for each occupied voxel with sufficient points. 464 | for (auto it = leaves_.begin (); it != leaves_.end (); ++it) 465 | { 466 | Leaf& leaf = it->second; 467 | 468 | if (leaf.nr_points >= min_points_per_voxel_) 469 | { 470 | cell_mean = leaf.mean_; 471 | llt_of_cov.compute (leaf.cov_); 472 | cholesky_decomp = llt_of_cov.matrixL (); 473 | 474 | // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix 475 | for (int i = 0; i < pnt_per_cell; i++) 476 | { 477 | rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ()); 478 | dist_point = cell_mean + cholesky_decomp * rand_point; 479 | cell_cloud.push_back (pcl::PointXYZ (static_cast (dist_point (0)), static_cast (dist_point (1)), static_cast (dist_point (2)))); 480 | } 481 | } 482 | } 483 | } 484 | 485 | #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; 486 | 487 | #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 488 | -------------------------------------------------------------------------------- /ndt_omp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ndt_omp 4 | 0.0.0 5 | OpenMP boosted NDT and GICP algorithms 6 | 7 | koide 8 | 9 | BSD 10 | 11 | catkin 12 | pcl_ros 13 | roscpp 14 | pcl_ros 15 | roscpp 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /ndt_omp/src/pclomp/gicp_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::GeneralizedIterativeClosestPoint; 5 | template class pclomp::GeneralizedIterativeClosestPoint; 6 | 7 | -------------------------------------------------------------------------------- /ndt_omp/src/pclomp/ndt_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::NormalDistributionsTransform; 5 | template class pclomp::NormalDistributionsTransform; 6 | -------------------------------------------------------------------------------- /ndt_omp/src/pclomp/voxel_grid_covariance_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class pclomp::VoxelGridCovariance; 5 | template class pclomp::VoxelGridCovariance; 6 | --------------------------------------------------------------------------------