From 40ac7a1a0d7a69e8e52b3631f4068a0a58d96e6b Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Fri, 1 Dec 2017 00:39:37 +0100 Subject: [PATCH 1/9] Extracted scan registration component into classes. * Extracted common scan registration logic into a ScanRegistration base class. This simplifies concrete implementations for different types of lidar sensors. * Extracted velodyne specific scan registration logic into VelodyneScanRegistration class. * Extracted remeining parts of main function into velodyne_scan_registration_node.cpp file. * Added struct for scan registration parameters. * Added struct for (accumulated) IMU states. * Created CircularBuffer class for holding the IMU state data history. * Moved some math functions from common.h to math_util.h. * Changed rotation functions to override input vector / point. Almost all rotations were performed on temporary data structures and then directly stored in their target data structure. Changes to the logic: * Calculate imuShiftFromStart and imuVelocityFromStart values only at the end of a sweep, as they are not used (more than once) inbetween. * Changed calculation of scan start and end indices, by using the individual ring cloud sizes. Bugfixes: * Fixed a copy & paste error in transformToStartIMU. New features: * All buffers within the scan registration components used during feature extration are now dynamic and automatically adapted during processing. Also, the buffer scopes - and with that their typical sizes - are reduced to their required minimum, to reduce the memory footprint. However, the number of scans per sweep in the VelodyneScanRegistration is still required to be 16, due to the scan ring mapping. * There exist two different modi: active and passtive. In active mode, the component works just like before. Passive mode is intended for offline processing of bag files and does not listen to topics nor publish any messages. --- .gitignore | 3 +- CMakeLists.txt | 12 +- include/loam_velodyne/common.h | 12 - src/laserMapping.cpp | 79 ++- src/laserOdometry.cpp | 162 ++---- src/lib/CMakeLists.txt | 8 + src/lib/CircularBuffer.h | 109 ++++ src/lib/ScanRegistration.cpp | 516 ++++++++++++++++++ src/lib/ScanRegistration.h | 411 ++++++++++++++ src/lib/VelodyneScanRegistration.cpp | 196 +++++++ src/lib/VelodyneScanRegistration.h | 80 +++ src/lib/math_utils.h | 399 ++++++++++++++ src/math_utils.h | 124 ----- src/scanRegistration.cpp | 692 ------------------------ src/velodyne_scan_registration_node.cpp | 20 + 15 files changed, 1843 insertions(+), 980 deletions(-) create mode 100644 src/lib/CMakeLists.txt create mode 100644 src/lib/CircularBuffer.h create mode 100644 src/lib/ScanRegistration.cpp create mode 100644 src/lib/ScanRegistration.h create mode 100644 src/lib/VelodyneScanRegistration.cpp create mode 100644 src/lib/VelodyneScanRegistration.h create mode 100644 src/lib/math_utils.h delete mode 100644 src/math_utils.h delete mode 100755 src/scanRegistration.cpp create mode 100644 src/velodyne_scan_registration_node.cpp diff --git a/.gitignore b/.gitignore index 7b4354f..1da4aac 100755 --- a/.gitignore +++ b/.gitignore @@ -3,8 +3,7 @@ build catkin devel catkin_generated -gtest -lib +gtest test_results CMakeFiles cmake_install.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 96df6e7..ce5e68c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,25 +10,29 @@ find_package(catkin REQUIRED COMPONENTS std_msgs tf) -#find_package(Eigen3 REQUIRED) +find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) include_directories( include ${catkin_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIR} + #${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS}) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs DEPENDS EIGEN3 PCL INCLUDE_DIRS include + LIBRARIES loam ) add_definitions( -march=native ) -add_executable(scanRegistration src/scanRegistration.cpp) -target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) + +add_subdirectory(src/lib) + +add_executable(scanRegistration src/velodyne_scan_registration_node.cpp) +target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) add_executable(laserOdometry src/laserOdometry.cpp) target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) diff --git a/include/loam_velodyne/common.h b/include/loam_velodyne/common.h index 67e5d87..d47348c 100644 --- a/include/loam_velodyne/common.h +++ b/include/loam_velodyne/common.h @@ -30,18 +30,6 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include - #include typedef pcl::PointXYZI PointType; - -inline double rad2deg(double radians) -{ - return radians * 180.0 / M_PI; -} - -inline double deg2rad(double degrees) -{ - return degrees * M_PI / 180.0; -} diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 1199cb3..d2ba4f9 100755 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -44,7 +44,9 @@ #include #include #include "loam_velodyne/nanoflann_pcl.h" -#include "math_utils.h" +#include "lib/math_utils.h" + +using namespace loam; const float scanPeriod = 0.1; @@ -106,10 +108,8 @@ float imuPitch[imuQueLength] = {0}; void transformAssociateToMap() { - Vector3 v0 = transformBefMapped.pos - transformSum.pos; - Vector3 v1 = rotateY( v0, -(transformSum.rot_y) ); - Vector3 v2 = rotateX( v1, -(transformSum.rot_x) ); - transformIncre.pos = rotateZ( v2, -(transformSum.rot_z) ); + transformIncre.pos = transformBefMapped.pos - transformSum.pos; + rotateYXZ(transformIncre.pos, -(transformSum.rot_y), -(transformSum.rot_x), -(transformSum.rot_z)); float sbcx = transformSum.rot_x.sin(); float cbcx = transformSum.rot_x.cos(); @@ -167,11 +167,9 @@ void transformAssociateToMap() transformTobeMapped.rot_z = atan2(srzcrx / transformTobeMapped.rot_x.cos(), crzcrx / transformTobeMapped.rot_x.cos()); - Vector3 v3; - v1 = rotateZ( transformIncre.pos, transformTobeMapped.rot_z); - v2 = rotateX( v1, transformTobeMapped.rot_x); - v3 = rotateY( v2, transformTobeMapped.rot_y); - transformTobeMapped.pos = transformAftMapped.pos - v3; + Vector3 v = transformIncre.pos; + rotateZXY(v, transformTobeMapped.rot_z, transformTobeMapped.rot_x, transformTobeMapped.rot_y); + transformTobeMapped.pos = transformAftMapped.pos - v; } void transformUpdate() @@ -199,8 +197,8 @@ void transformUpdate() imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; } - transformTobeMapped.rot_x = 0.998 * transformTobeMapped.rot_x.value() + 0.002 * imuPitchLast; - transformTobeMapped.rot_z = 0.998 * transformTobeMapped.rot_z.value() + 0.002 * imuRollLast; + transformTobeMapped.rot_x = 0.998 * transformTobeMapped.rot_x.rad() + 0.002 * imuPitchLast; + transformTobeMapped.rot_z = 0.998 * transformTobeMapped.rot_z.rad() + 0.002 * imuRollLast; } transformBefMapped = transformSum; @@ -209,28 +207,26 @@ void transformUpdate() void pointAssociateToMap(PointType const * const pi, PointType * const po) { - Vector3 v1 = rotateZ( *pi, transformTobeMapped.rot_z); - Vector3 v2 = rotateX( v1, transformTobeMapped.rot_x); - Vector3 v3 = rotateY( v2, transformTobeMapped.rot_y); - v3 += transformTobeMapped.pos; - - po->x = v3.x(); - po->y = v3.y(); - po->z = v3.z(); + po->x = pi->x; + po->y = pi->y; + po->z = pi->z; po->intensity = pi->intensity; + + rotateZXY(*po, transformTobeMapped.rot_z, transformTobeMapped.rot_x, transformTobeMapped.rot_y); + + po->x += transformTobeMapped.pos.x(); + po->y += transformTobeMapped.pos.y(); + po->z += transformTobeMapped.pos.z(); } void pointAssociateTobeMapped(PointType const * const pi, PointType * const po) { - Vector3 v0 = Vector3(*pi) - transformTobeMapped.pos; - Vector3 v1 = rotateY( v0, -transformTobeMapped.rot_y); - Vector3 v2 = rotateX( v1, -transformTobeMapped.rot_x); - Vector3 v3 = rotateZ( v2, -transformTobeMapped.rot_z); - - po->x = v3.x(); - po->y = v3.y(); - po->z = v3.z(); + po->x = pi->x - transformTobeMapped.pos.x(); + po->y = pi->y - transformTobeMapped.pos.y(); + po->z = pi->z - transformTobeMapped.pos.z(); po->intensity = pi->intensity; + + rotateYXZ(*po, -transformTobeMapped.rot_y, -transformTobeMapped.rot_x, -transformTobeMapped.rot_z); } void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2) @@ -595,18 +591,19 @@ int main(int argc, char** argv) float centerY = 50.0 * (j - laserCloudCenHeight); float centerZ = 50.0 * (k - laserCloudCenDepth); + PointType transform_pos = (pcl::PointXYZI) transformTobeMapped.pos; + bool isInLaserFOV = false; for (int ii = -1; ii <= 1; ii += 2) { for (int jj = -1; jj <= 1; jj += 2) { for (int kk = -1; kk <= 1; kk += 2) { - Vector3 corner; - corner.x() = centerX + 25.0 * ii; - corner.y() = centerY + 25.0 * jj; - corner.z() = centerZ + 25.0 * kk; + PointType corner; + corner.x = centerX + 25.0 * ii; + corner.y = centerY + 25.0 * jj; + corner.z = centerZ + 25.0 * kk; - Vector3 point_on_axis( pointOnYAxis.x, pointOnYAxis.y, pointOnYAxis.z); - float squaredSide1 = (transformTobeMapped.pos - corner).squaredNorm(); - float squaredSide2 = (point_on_axis - corner).squaredNorm(); + float squaredSide1 = calcSquaredDiff(transform_pos, corner); + float squaredSide2 = calcSquaredDiff(pointOnYAxis, corner); float check1 = 100.0 + squaredSide1 - squaredSide2 - 10.0 * sqrt(3.0) * sqrt(squaredSide1); @@ -1023,9 +1020,9 @@ int main(int argc, char** argv) pubLaserCloudFullRes.publish(laserCloudFullRes3); geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw - ( transformAftMapped.rot_z.value(), - -transformAftMapped.rot_x.value(), - -transformAftMapped.rot_y.value()); + ( transformAftMapped.rot_z.rad(), + -transformAftMapped.rot_x.rad(), + -transformAftMapped.rot_y.rad()); odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); odomAftMapped.pose.pose.orientation.x = -geoQuat.y; @@ -1035,9 +1032,9 @@ int main(int argc, char** argv) odomAftMapped.pose.pose.position.x = transformAftMapped.pos.x(); odomAftMapped.pose.pose.position.y = transformAftMapped.pos.y(); odomAftMapped.pose.pose.position.z = transformAftMapped.pos.z(); - odomAftMapped.twist.twist.angular.x = transformBefMapped.rot_x.value(); - odomAftMapped.twist.twist.angular.y = transformBefMapped.rot_y.value(); - odomAftMapped.twist.twist.angular.z = transformBefMapped.rot_z.value(); + odomAftMapped.twist.twist.angular.x = transformBefMapped.rot_x.rad(); + odomAftMapped.twist.twist.angular.y = transformBefMapped.rot_y.rad(); + odomAftMapped.twist.twist.angular.z = transformBefMapped.rot_z.rad(); odomAftMapped.twist.twist.linear.x = transformBefMapped.pos.x(); odomAftMapped.twist.twist.linear.y = transformBefMapped.pos.y(); odomAftMapped.twist.twist.linear.z = transformBefMapped.pos.z(); diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp index cf622f4..e1282a2 100755 --- a/src/laserOdometry.cpp +++ b/src/laserOdometry.cpp @@ -30,8 +30,11 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include +#include "loam_velodyne/nanoflann_pcl.h" +#include "lib/math_utils.h" + +#include #include #include #include @@ -45,8 +48,8 @@ #include #include #include -#include "loam_velodyne/nanoflann_pcl.h" -#include "math_utils.h" + +using namespace loam; const float scanPeriod = 0.1; @@ -106,51 +109,38 @@ void TransformToStart(PointType const * const pi, PointType * const po) { float s = 10 * (pi->intensity - int(pi->intensity)); - Angle rx = s * transform.rot_x.value(); - Angle ry = s * transform.rot_y.value(); - Angle rz = s * transform.rot_z.value(); - - Vector3 v0( Vector3(*pi) -s * transform.pos ); - Vector3 v1 = rotateZ( v0, -rz ); - Vector3 v2 = rotateX( v1, -rx ); - Vector3 v3 = rotateY( v2, -ry ); - - po->x = v3.x(); - po->y = v3.y(); - po->z = v3.z(); + po->x = pi->x - s * transform.pos.x(); + po->y = pi->y - s * transform.pos.y(); + po->z = pi->z - s * transform.pos.z(); po->intensity = pi->intensity; + + Angle rx = s * transform.rot_x.rad(); + Angle ry = s * transform.rot_y.rad(); + Angle rz = s * transform.rot_z.rad(); + rotateZXY(*po, -rz, -rx, -ry); } void TransformToEnd(PointType const * const pi, PointType * const po) { float s = 10 * (pi->intensity - int(pi->intensity)); - Angle rx = s * transform.rot_x.value(); - Angle ry = s * transform.rot_y.value(); - Angle rz = s * transform.rot_z.value(); - - Vector3 v0( Vector3(*pi) -s * transform.pos ); - Vector3 v1 = rotateZ( v0, -rz ); - Vector3 v2 = rotateX( v1, -rx ); - Vector3 v3 = rotateY( v2, -ry ); - - Vector3 v4 = rotateY( v3, transform.rot_y ); - Vector3 v5 = rotateX( v4, transform.rot_x ); - Vector3 v6 = rotateZ( v5, transform.rot_z ); - v6 += transform.pos - imuShiftFromStart; + po->x = pi->x - s * transform.pos.x(); + po->y = pi->y - s * transform.pos.y(); + po->z = pi->z - s * transform.pos.z(); + po->intensity = int(pi->intensity); - Vector3 v7 = rotateZ( v6, imuRollStart ); - Vector3 v8 = rotateX( v7, imuPitchStart ); - Vector3 v9 = rotateY( v8, imuYawStart ); + Angle rx = s * transform.rot_x.rad(); + Angle ry = s * transform.rot_y.rad(); + Angle rz = s * transform.rot_z.rad(); + rotateZXY(*po, -rz, -rx, -ry); + rotateYXZ(*po, transform.rot_y, transform.rot_x, transform.rot_z); - Vector3 v10 = rotateY( v9, -imuYawLast ); - Vector3 v11 = rotateX( v10, -imuPitchLast ); - Vector3 v12 = rotateZ( v11, -imuRollLast ); + po->x += transform.pos.x() - imuShiftFromStart.x(); + po->y += transform.pos.y() - imuShiftFromStart.y(); + po->z += transform.pos.z() - imuShiftFromStart.z(); - po->x = v12.x(); - po->y = v12.y(); - po->z = v12.z(); - po->intensity = int(pi->intensity); + rotateZXY(*po, imuRollStart, imuPitchStart, imuYawStart); + rotateYXZ(*po, -imuYawLast, -imuPitchLast, -imuRollLast); } void PluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, @@ -305,13 +295,8 @@ void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) imuYawLast = imuTrans->points[1].y; imuRollLast = imuTrans->points[1].z; - imuShiftFromStart.x() = imuTrans->points[2].x; - imuShiftFromStart.y() = imuTrans->points[2].y; - imuShiftFromStart.z() = imuTrans->points[2].z; - - imuVeloFromStart.x() = imuTrans->points[3].x; - imuVeloFromStart.y() = imuTrans->points[3].y; - imuVeloFromStart.z() = imuTrans->points[3].z; + imuShiftFromStart = imuTrans->points[2]; + imuVeloFromStart = imuTrans->points[3]; newImuTrans = true; } @@ -385,13 +370,8 @@ int main(int argc, char** argv) newImuTrans = false; if (!systemInited) { - pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; - cornerPointsLessSharp = laserCloudCornerLast; - laserCloudCornerLast = laserCloudTemp; - - laserCloudTemp = surfPointsLessFlat; - surfPointsLessFlat = laserCloudSurfLast; - laserCloudSurfLast = laserCloudTemp; + std::swap(cornerPointsLessSharp, laserCloudCornerLast); + std::swap(surfPointsLessFlat, laserCloudSurfLast); sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); @@ -450,12 +430,7 @@ int main(int argc, char** argv) break; } - pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * - (laserCloudCornerLast->points[j].x - pointSel.x) + - (laserCloudCornerLast->points[j].y - pointSel.y) * - (laserCloudCornerLast->points[j].y - pointSel.y) + - (laserCloudCornerLast->points[j].z - pointSel.z) * - (laserCloudCornerLast->points[j].z - pointSel.z); + pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) { if (pointSqDis < minPointSqDis2) { @@ -469,12 +444,7 @@ int main(int argc, char** argv) break; } - pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * - (laserCloudCornerLast->points[j].x - pointSel.x) + - (laserCloudCornerLast->points[j].y - pointSel.y) * - (laserCloudCornerLast->points[j].y - pointSel.y) + - (laserCloudCornerLast->points[j].z - pointSel.z) * - (laserCloudCornerLast->points[j].z - pointSel.z); + pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) { if (pointSqDis < minPointSqDis2) { @@ -561,12 +531,7 @@ int main(int argc, char** argv) break; } - pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * - (laserCloudSurfLast->points[j].x - pointSel.x) + - (laserCloudSurfLast->points[j].y - pointSel.y) * - (laserCloudSurfLast->points[j].y - pointSel.y) + - (laserCloudSurfLast->points[j].z - pointSel.z) * - (laserCloudSurfLast->points[j].z - pointSel.z); + pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) { if (pointSqDis < minPointSqDis2) { @@ -585,12 +550,7 @@ int main(int argc, char** argv) break; } - pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * - (laserCloudSurfLast->points[j].x - pointSel.x) + - (laserCloudSurfLast->points[j].y - pointSel.y) * - (laserCloudSurfLast->points[j].y - pointSel.y) + - (laserCloudSurfLast->points[j].z - pointSel.z) * - (laserCloudSurfLast->points[j].z - pointSel.z); + pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) { if (pointSqDis < minPointSqDis2) { @@ -673,12 +633,12 @@ int main(int argc, char** argv) float s = 1; - float srx = sin(s * transform.rot_x.value()); - float crx = cos(s * transform.rot_x.value()); - float sry = sin(s * transform.rot_y.value()); - float cry = cos(s * transform.rot_y.value()); - float srz = sin(s * transform.rot_z.value()); - float crz = cos(s * transform.rot_z.value()); + float srx = sin(s * transform.rot_x.rad()); + float crx = cos(s * transform.rot_x.rad()); + float sry = sin(s * transform.rot_y.rad()); + float cry = cos(s * transform.rot_y.rad()); + float srz = sin(s * transform.rot_z.rad()); + float crz = cos(s * transform.rot_z.rad()); float tx = s * transform.pos.x(); float ty = s * transform.pos.y(); float tz = s * transform.pos.z(); @@ -762,16 +722,16 @@ int main(int argc, char** argv) matX = matP * matX2; } - transform.rot_x = transform.rot_x.value() + matX(0, 0); - transform.rot_y = transform.rot_y.value() + matX(1, 0); - transform.rot_z = transform.rot_z.value() + matX(2, 0); + transform.rot_x = transform.rot_x.rad() + matX(0, 0); + transform.rot_y = transform.rot_y.rad() + matX(1, 0); + transform.rot_z = transform.rot_z.rad() + matX(2, 0); transform.pos.x() += matX(3, 0); transform.pos.y() += matX(4, 0); transform.pos.z() += matX(5, 0); - if( isnan(transform.rot_x.value()) ) transform.rot_x = Angle(); - if( isnan(transform.rot_y.value()) ) transform.rot_y = Angle(); - if( isnan(transform.rot_z.value()) ) transform.rot_z = Angle(); + if( isnan(transform.rot_x.rad()) ) transform.rot_x = Angle(); + if( isnan(transform.rot_y.rad()) ) transform.rot_y = Angle(); + if( isnan(transform.rot_z.rad()) ) transform.rot_z = Angle(); if( isnan(transform.pos.x()) ) transform.pos.x() = 0.0; if( isnan(transform.pos.y()) ) transform.pos.y() = 0.0; @@ -798,18 +758,15 @@ int main(int argc, char** argv) transformSum.rot_y, transformSum.rot_z, -transform.rot_x, - -transform.rot_y.value() * 1.05, + -transform.rot_y.rad() * 1.05, -transform.rot_z, rx, ry, rz); - Vector3 v0( transform.pos.x() - imuShiftFromStart.x(), - transform.pos.y() - imuShiftFromStart.y(), - transform.pos.z()*1.05 - imuShiftFromStart.z() ); - - Vector3 v1 = rotateZ( v0, rz ); - Vector3 v2 = rotateX( v1, rx ); - Vector3 v3 = rotateY( v2, ry ); - Vector3 trans = transformSum.pos - v3; + Vector3 v( transform.pos.x() - imuShiftFromStart.x(), + transform.pos.y() - imuShiftFromStart.y(), + transform.pos.z()*1.05 - imuShiftFromStart.z() ); + rotateZXY(v, rz, rx, ry); + Vector3 trans = transformSum.pos - v; PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, @@ -821,7 +778,7 @@ int main(int argc, char** argv) transformSum.rot_z = rz; transformSum.pos = trans; - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.value(), -rx.value(), -ry.value()); + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserOdometry.pose.pose.orientation.x = -geoQuat.y; @@ -856,13 +813,8 @@ int main(int argc, char** argv) } } - pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; - cornerPointsLessSharp = laserCloudCornerLast; - laserCloudCornerLast = laserCloudTemp; - - laserCloudTemp = surfPointsLessFlat; - surfPointsLessFlat = laserCloudSurfLast; - laserCloudSurfLast = laserCloudTemp; + std::swap(cornerPointsLessSharp, laserCloudCornerLast); + std::swap(surfPointsLessFlat, laserCloudSurfLast); laserCloudCornerLastNum = laserCloudCornerLast->points.size(); laserCloudSurfLastNum = laserCloudSurfLast->points.size(); diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt new file mode 100644 index 0000000..5350aa7 --- /dev/null +++ b/src/lib/CMakeLists.txt @@ -0,0 +1,8 @@ +add_library(loam + math_utils.h + CircularBuffer.h + ScanRegistration.h + ScanRegistration.cpp + VelodyneScanRegistration.h + VelodyneScanRegistration.cpp) +target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/CircularBuffer.h b/src/lib/CircularBuffer.h new file mode 100644 index 0000000..60e1f4a --- /dev/null +++ b/src/lib/CircularBuffer.h @@ -0,0 +1,109 @@ +#ifndef LOAM_CIRCULARBUFFER_H +#define LOAM_CIRCULARBUFFER_H + +#include +#include + + +namespace loam { + + + +/** \brief Simple circular buffer implementation for storing data history. + * + * @tparam T The buffer element type. + */ +template +class CircularBuffer { +public: + CircularBuffer(const size_t& capacity = 200) + : _capacity(capacity), + _size(0), + _startIdx(0) + { + _buffer = new T[capacity]; + }; + + ~CircularBuffer() + { + delete[] _buffer; + _buffer = NULL; + } + + /** \brief Retrieve the buffer size. + * + * @return the buffer size + */ + const size_t& size() { + return _size; + } + + /** \brief Retrieve the buffer capacity. + * + * @return the buffer capacity + */ + const size_t& capacity() { + return _capacity; + } + + /** \brief Check if the buffer is empty. + * + * @return true if the buffer is empty, false otherwise + */ + bool empty() { + return _size == 0; + } + + /** \brief Retrieve the i-th element of the buffer. + * + * + * @param i the buffer index + * @return the element at the i-th position + */ + const T& operator[](const size_t& i) { + return _buffer[(_startIdx + i) % _capacity]; + } + + /** \brief Retrieve the first (oldest) element of the buffer. + * + * @return the first element + */ + const T& first() { + return _buffer[_startIdx]; + } + + /** \brief Retrieve the last (latest) element of the buffer. + * + * @return the last element + */ + const T& last() { + size_t idx = _size == 0 ? 0 : (_startIdx + _size - 1) % _capacity; + return _buffer[idx]; + } + + /** \brief Push a new element to the buffer. + * + * If the buffer reached its capacity, the oldest element is overwritten. + * + * @param element the element to push + */ + void push(const T& element) { + if (_size < _capacity) { + _buffer[_size] = element; + _size++; + } else { + _buffer[_startIdx] = element; + _startIdx = (_startIdx + 1) % _capacity; + } + } + +private: + size_t _capacity; ///< buffer capacity + size_t _size; ///< current buffer size + size_t _startIdx; ///< current start index + T* _buffer; ///< internal element buffer +}; + +} // end namespace loam + +#endif //LOAM_CIRCULARBUFFER_H diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp new file mode 100644 index 0000000..3aeff13 --- /dev/null +++ b/src/lib/ScanRegistration.cpp @@ -0,0 +1,516 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "ScanRegistration.h" + +#include +#include + + +namespace loam { + +ScanRegistration::ScanRegistration(const float& scanPeriod, + const uint16_t& nScans, + const size_t& imuHistorySize, + const RegistrationParams& config) + : _nScans(nScans), + _scanPeriod(scanPeriod), + _sweepStamp(), + _config(config), + _imuStart(), + _imuCur(), + _imuStartIdx(0), + _imuHistory(imuHistorySize), + _laserCloud(new pcl::PointCloud()), + _cornerPointsSharp(new pcl::PointCloud()), + _cornerPointsLessSharp(new pcl::PointCloud()), + _surfacePointsFlat(new pcl::PointCloud()), + _surfacePointsLessFlat(new pcl::PointCloud()), + _imuTrans(new pcl::PointCloud(4, 1)), + _regionCurvature(), + _regionLabel(), + _regionSortIndices(), + _scanNeighborPicked(), + _activeMode(false) +{ + _scanStartIndices.assign(nScans, 0); + _scanEndIndices.assign(nScans, 0); +}; + + + +bool ScanRegistration::setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode) +{ + if (!_config.parseParams(privateNode)) { + return false; + } + _config.print(); + + // subscribe to IMU topic + _subImu = node.subscribe("/imu/data", 50, &ScanRegistration::handleIMUMessage, this); + + + // advertise scan registration topics + _pubLaserCloud = node.advertise("/velodyne_cloud_2", 2); + _pubCornerPointsSharp = node.advertise("/laser_cloud_sharp", 2); + _pubCornerPointsLessSharp = node.advertise("/laser_cloud_less_sharp", 2); + _pubSurfPointsFlat = node.advertise("/laser_cloud_flat", 2); + _pubSurfPointsLessFlat = node.advertise("/laser_cloud_less_flat", 2); + _pubImuTrans = node.advertise("/imu_trans", 5); + + // set active mode + _activeMode = true; + + return true; +} + + + +bool ScanRegistration::setup(const RegistrationParams& config) +{ + _config = config; + _config.print(); + + return true; +} + + + +void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn) +{ + double roll, pitch, yaw; + tf::Quaternion orientation; + tf::quaternionMsgToTF(imuIn->orientation, orientation); + tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + + Vector3 acc; + acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81); + acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81); + acc.z() = float(imuIn->linear_acceleration.x + sin(pitch) * 9.81); + + IMUState newState; + newState.stamp = imuIn->header.stamp; + newState.roll = roll; + newState.pitch = pitch; + newState.yaw = yaw; + newState.acceleration = acc; + + if (_imuHistory.size() > 0) { + // accumulate IMU position and velocity over time + rotateZXY(acc, newState.roll, newState.pitch, newState.yaw); + + const IMUState& prevState = _imuHistory.last(); + float timeDiff = float((newState.stamp - prevState.stamp).toSec()); + newState.position = prevState.position + + (prevState.velocity * timeDiff) + + (0.5 * acc * timeDiff * timeDiff); + newState.velocity = prevState.velocity + + acc * timeDiff; + } + + _imuHistory.push(newState); +} + + + +void ScanRegistration::reset(const ros::Time& scanTime) +{ + _sweepStamp = scanTime; + + // clear cloud buffers + _laserCloud->clear(); + _cornerPointsSharp->clear(); + _cornerPointsLessSharp->clear(); + _surfacePointsFlat->clear(); + _surfacePointsLessFlat->clear(); + + + // reset scan indices vectors + _scanStartIndices.assign(_nScans, 0); + _scanEndIndices.assign(_nScans, 0); + + + // re-initialize IMU start state and index + _imuStartIdx = 0; + + if (_imuHistory.size() > 0) { + while (_imuStartIdx < _imuHistory.size() - 1 && (scanTime - _imuHistory[_imuStartIdx].stamp).toSec() > 0) { + _imuStartIdx++; + } + + // fetch / interpolate IMU start state + if (_imuStartIdx == 0 || (scanTime - _imuHistory[_imuStartIdx].stamp).toSec() > 0) { + // scan time newer then newest or older than oldest IMU message + _imuStart = _imuHistory[_imuStartIdx]; + } else { + float ratio = (_imuHistory[_imuStartIdx].stamp - scanTime).toSec() + / (_imuHistory[_imuStartIdx].stamp - _imuHistory[_imuStartIdx - 1].stamp).toSec(); + IMUState::interpolate(_imuHistory[_imuStartIdx], _imuHistory[_imuStartIdx - 1], ratio, _imuStart); + } + } +} + + + +void ScanRegistration::transformToStartIMU(pcl::PointXYZI& point, + const float& pointTime) +{ + // rotate point to global IMU system + rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw); + + // add global IMU position shift + Vector3 positionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * pointTime; + point.x += positionShift.x(); + point.y += positionShift.y(); + point.z += positionShift.z(); + + // rotate point back to local IMU system relative to the start IMU state + rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); +} + + + +void ScanRegistration::setIMUTrans(const double& sweepDuration) +{ + _imuTrans->points[0].x = _imuStart.pitch.rad(); + _imuTrans->points[0].y = _imuStart.yaw.rad(); + _imuTrans->points[0].z = _imuStart.roll.rad(); + + _imuTrans->points[1].x = _imuCur.pitch.rad(); + _imuTrans->points[1].y = _imuCur.yaw.rad(); + _imuTrans->points[1].z = _imuCur.roll.rad(); + + Vector3 imuShiftFromStart = _imuCur.position - _imuStart.position - _imuStart.velocity * sweepDuration; + rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); + + _imuTrans->points[2].x = imuShiftFromStart.x(); + _imuTrans->points[2].y = imuShiftFromStart.y(); + _imuTrans->points[2].z = imuShiftFromStart.z(); + + Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity; + rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); + + _imuTrans->points[3].x = imuVelocityFromStart.x(); + _imuTrans->points[3].y = imuVelocityFromStart.y(); + _imuTrans->points[3].z = imuVelocityFromStart.z(); +} + + + +void ScanRegistration::extractFeatures(const uint16_t& beginIdx) +{ + // extract features from individual scans + for (int i = beginIdx; i < _nScans; i++) { + // ROS_INFO("Extract features for scan %d", i); + + pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); + size_t scanStartIdx = _scanStartIndices[i]; + size_t scanEndIdx = _scanEndIndices[i]; + + // skip empty scans + if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) { + continue; + } + + // Quick&Dirty fix for relative point time calculation without IMU data + /*float scanSize = scanEndIdx - scanStartIdx + 1; + for (int j = scanStartIdx; j <= scanEndIdx; j++) { + _laserCloud->points[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize; + }*/ + + // reset scan buffers + setScanBuffersFor(scanStartIdx, scanEndIdx); + + // extract features from equally sized scan regions + for (int j = 0; j < _config.nFeatureRegions; j++) { + size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j) + + (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions; + size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j) + + (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1; + + // skip empty regions + if (ep <= sp) { + continue; + } + + size_t regionSize = ep - sp + 1; + + // reset region buffers + setRegionBuffersFor(sp, ep); + + + // ROS_INFO("Extract corner features"); + + // extract corner features + int largestPickedNum = 0; + for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) { + size_t idx = _regionSortIndices[--k]; + size_t scanIdx = idx - scanStartIdx; + size_t regionIdx = idx - sp; + + if (_scanNeighborPicked[scanIdx] == 0 && + _regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) { + + largestPickedNum++; + if (largestPickedNum <= _config.maxCornerSharp) { + _regionLabel[regionIdx] = CORNER_SHARP; + _cornerPointsSharp->push_back(_laserCloud->points[idx]); + _cornerPointsLessSharp->push_back(_laserCloud->points[idx]); + } else { + _regionLabel[regionIdx] = CORNER_LESS_SHARP; + _cornerPointsLessSharp->push_back(_laserCloud->points[idx]); + } + + _scanNeighborPicked[scanIdx] = 1; + for (int l = 1; l <= _config.curvatureRegion; l++) { + float distSquare = calcSquaredDiff(_laserCloud->points[idx + l], _laserCloud->points[idx + l - 1]); + if (distSquare > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx + l] = 1; + } + for (int l = 1; l <= _config.curvatureRegion; l++) { + float distSquare = calcSquaredDiff(_laserCloud->points[idx - l], _laserCloud->points[idx - l + 1]); + if (distSquare > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx - l] = 1; + } + } + } + + // ROS_INFO("Extract flat features"); + + // extract flat surface features + int smallestPickedNum = 0; + for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) { + size_t idx = _regionSortIndices[k]; + size_t scanIdx = idx - scanStartIdx; + size_t regionIdx = idx - sp; + + if (_scanNeighborPicked[scanIdx] == 0 && + _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) { + + smallestPickedNum++; + _regionLabel[regionIdx] = SURFACE_FLAT; + _surfacePointsFlat->push_back(_laserCloud->points[idx]); + + _scanNeighborPicked[scanIdx] = 1; + for (int l = 1; l <= _config.curvatureRegion; l++) { + float distSquare = calcSquaredDiff(_laserCloud->points[idx + l], _laserCloud->points[idx + l - 1]); + if (distSquare > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx + l] = 1; + } + for (int l = 1; l <= _config.curvatureRegion; l++) { + float distSquare = calcSquaredDiff(_laserCloud->points[idx - l], _laserCloud->points[idx - l + 1]); + if (distSquare > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx - l] = 1; + } + } + } + + // ROS_INFO("Extract less flat features"); + + // extract less flat surface features + for (int k = 0; k < regionSize; k++) { + if (_regionLabel[k] <= SURFACE_LESS_FLAT) { + surfPointsLessFlatScan->push_back(_laserCloud->points[sp + k]); + } + } + } + + // down size less flat surface point cloud of current scan + pcl::PointCloud surfPointsLessFlatScanDS; + pcl::VoxelGrid downSizeFilter; + downSizeFilter.setInputCloud(surfPointsLessFlatScan); + downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize); + downSizeFilter.filter(surfPointsLessFlatScanDS); + + *_surfacePointsLessFlat += surfPointsLessFlatScanDS; + } +} + + + +void ScanRegistration::setRegionBuffersFor(const size_t& startIdx, + const size_t& endIdx) +{ + // ROS_INFO("Set region buffers for %d to %d", int(startIdx), int(endIdx)); + + // resize buffers + size_t regionSize = endIdx - startIdx + 1; + _regionCurvature.resize(regionSize); + _regionSortIndices.resize(regionSize); + _regionLabel.assign(regionSize, SURFACE_LESS_FLAT); + + // calculate point curvatures and reset sort indices + float pointWeight = -2 * _config.curvatureRegion; + + for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) { + float diffX = pointWeight * _laserCloud->points[i].x; + float diffY = pointWeight * _laserCloud->points[i].y; + float diffZ = pointWeight * _laserCloud->points[i].z; + + for (int j = 1; j <= _config.curvatureRegion; j++) { + diffX += _laserCloud->points[i + j].x + _laserCloud->points[i - j].x; + diffY += _laserCloud->points[i + j].y + _laserCloud->points[i - j].y; + diffZ += _laserCloud->points[i + j].z + _laserCloud->points[i - j].z; + } + + _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ; + _regionSortIndices[regionIdx] = i; + } + + // sort point curvatures + for (size_t i = 1; i < regionSize; i++) { + for (size_t j = i; j >= 1; j--) { + if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) { + std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]); + } + } + } +} + + + +void ScanRegistration::setScanBuffersFor(const size_t& startIdx, + const size_t& endIdx) +{ + // ROS_INFO("Set scan buffers for %d to %d", int(startIdx), int(endIdx)); + + // resize buffers + size_t scanSize = endIdx - startIdx + 1; + _scanNeighborPicked.assign(scanSize, 0); + + // mark unreliable points as picked + for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) { + const PointType& prevPoint = (_laserCloud->points[i - 1]); + const PointType& point = (_laserCloud->points[i]); + const PointType& nextPoint = (_laserCloud->points[i + 1]); + + float diff = calcSquaredDiff(nextPoint, point); + + if (diff > 0.1) { + float depth1 = calcPointDistance(point); + float depth2 = calcPointDistance(nextPoint); + + if (depth1 > depth2) { + float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2; + + if (weighted_distance < 0.1) { + size_t scanIdx = i - startIdx; + for (int j = 0; j <= _config.curvatureRegion; j++) { + _scanNeighborPicked[scanIdx - j] = 1; + } + + continue; + } + } else { + float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1; + + if (weighted_distance < 0.1) { + size_t scanIdx = i - startIdx; + for (int j = _config.curvatureRegion + 1; j > 0 ; j--) { + _scanNeighborPicked[scanIdx + j] = 1; + } + } + } + } + + float diff2 = calcSquaredDiff(point, prevPoint); + float dis = calcSquaredPointDistance(point); + + if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { + _scanNeighborPicked[i - startIdx] = 1; + } + } +} + + + +void ScanRegistration::generateROSMsg(sensor_msgs::PointCloud2& msg, + const pcl::PointCloud::Ptr& cloud) +{ + pcl::toROSMsg(*cloud, msg); + msg.header.stamp = _sweepStamp; + msg.header.frame_id = "/camera"; +} + + + +void ScanRegistration::publishResult() +{ + if (!_activeMode) { + // only publish messages in active mode + return; + } + + // publish full resolution and feature point clouds + sensor_msgs::PointCloud2 laserCloudOutMsg; + generateROSMsg(laserCloudOutMsg, _laserCloud); + _pubLaserCloud.publish(laserCloudOutMsg); + + sensor_msgs::PointCloud2 cornerPointsSharpMsg; + generateROSMsg(cornerPointsSharpMsg, _cornerPointsSharp); + _pubCornerPointsSharp.publish(cornerPointsSharpMsg); + + sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; + generateROSMsg(cornerPointsLessSharpMsg, _cornerPointsLessSharp); + _pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); + + sensor_msgs::PointCloud2 surfPointsFlat; + generateROSMsg(surfPointsFlat, _surfacePointsFlat); + _pubSurfPointsFlat.publish(surfPointsFlat); + + sensor_msgs::PointCloud2 surfPointsLessFlat; + generateROSMsg(surfPointsLessFlat, _surfacePointsLessFlat); + _pubSurfPointsLessFlat.publish(surfPointsLessFlat); + + + // publish corresponding IMU transformation information + sensor_msgs::PointCloud2 imuTransMsg; + pcl::toROSMsg(*_imuTrans, imuTransMsg); + imuTransMsg.header.stamp = _sweepStamp; + imuTransMsg.header.frame_id = "/camera"; + _pubImuTrans.publish(imuTransMsg); +} + +} // end namespace loam diff --git a/src/lib/ScanRegistration.h b/src/lib/ScanRegistration.h new file mode 100644 index 0000000..db6f677 --- /dev/null +++ b/src/lib/ScanRegistration.h @@ -0,0 +1,411 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_SCANREGISTRATION_H +#define LOAM_SCANREGISTRATION_H + + +#include "CircularBuffer.h" +#include "math_utils.h" + +#include + +#include +#include +#include +#include +#include +#include + + +namespace loam { + +/** Point label options. */ +enum PointLabel { + CORNER_SHARP = 2, ///< sharp corner point + CORNER_LESS_SHARP = 1, ///< less sharp corner point + SURFACE_LESS_FLAT = 0, ///< less flat surface point + SURFACE_FLAT = -1 ///< flat surface point +}; + + + +/** Scan Registration configuration parameters. */ +typedef struct RegistrationParams { + RegistrationParams(int nFeatureRegions_ = 6, + int curvatureRegion_ = 5, + int maxCornerSharp_ = 2, + int maxSurfaceFlat_ = 4, + float lessFlatFilterSize_ = 0.2, + float surfaceCurvatureThreshold_ = 0.1) + : nFeatureRegions(nFeatureRegions_), + curvatureRegion(curvatureRegion_), + maxCornerSharp(maxCornerSharp_), + maxCornerLessSharp(10 * maxCornerSharp_), + maxSurfaceFlat(maxSurfaceFlat_), + lessFlatFilterSize(lessFlatFilterSize_), + surfaceCurvatureThreshold(surfaceCurvatureThreshold_) {}; + + ~RegistrationParams() {}; + + /** The number of (equally sized) regions used to distribute the feature extraction within a scan. */ + int nFeatureRegions; + + /** The number of surrounding points (+/- region around a point) used to calculate a point curvature. */ + int curvatureRegion; + + /** The maximum number of sharp corner points per feature region. */ + int maxCornerSharp; + + /** The maximum number of less sharp corner points per feature region. */ + int maxCornerLessSharp; + + /** The maximum number of flat surface points per feature region. */ + int maxSurfaceFlat; + + /** The voxel size used for down sizing the remaining less flat surface points. */ + float lessFlatFilterSize; + + /** The curvature threshold below / above a point is considered a flat / corner point. */ + float surfaceCurvatureThreshold; + + + /** \brief Parse node parameter. + * + * @param nh the ROS node handle + * @return true, if all specified parameters are valid, false if at least one specified parameter is invalid + */ + bool parseParams(const ros::NodeHandle& nh) { + bool success = true; + int iParam = 0; + float fParam = 0; + + if (nh.getParam("featureRegions", iParam)) { + if (iParam < 1) { + ROS_ERROR("Invalid featureRegions parameter: %d (expected >= 1)", iParam); + success = false; + } else { + nFeatureRegions = iParam; + } + } + + if (nh.getParam("curvatureRegion", iParam)) { + if (iParam < 1) { + ROS_ERROR("Invalid curvatureRegion parameter: %d (expected >= 1)", iParam); + success = false; + } else { + curvatureRegion = iParam; + } + } + + if (nh.getParam("maxCornerSharp", iParam)) { + if (iParam < 1) { + ROS_ERROR("Invalid maxCornerSharp parameter: %d (expected >= 1)", iParam); + success = false; + } else { + maxCornerSharp = iParam; + maxCornerLessSharp = 10 * iParam; + } + } + + if (nh.getParam("maxCornerLessSharp", iParam)) { + if (iParam < maxCornerSharp) { + ROS_ERROR("Invalid maxCornerLessSharp parameter: %d (expected >= %d)", iParam, maxCornerSharp); + success = false; + } else { + maxCornerLessSharp = iParam; + } + } + + if (nh.getParam("maxSurfaceFlat", iParam)) { + if (iParam < 1) { + ROS_ERROR("Invalid maxSurfaceFlat parameter: %d (expected >= 1)", iParam); + success = false; + } else { + maxSurfaceFlat = iParam; + } + } + + if (nh.getParam("surfaceCurvatureThreshold", fParam)) { + if (fParam < 0.001) { + ROS_ERROR("Invalid surfaceCurvatureThreshold parameter: %f (expected >= 0.001)", fParam); + success = false; + } else { + surfaceCurvatureThreshold = fParam; + } + } + + if (nh.getParam("lessFlatFilterSize", fParam)) { + if (fParam < 0.001) { + ROS_ERROR("Invalid lessFlatFilterSize parameter: %f (expected >= 0.001)", fParam); + success = false; + } else { + lessFlatFilterSize = fParam; + } + } + + return success; + }; + + /** \brief Print parameters to ROS_INFO. */ + void print() + { + ROS_INFO_STREAM(" ===== scan registration parameters =====" << std::endl + << " - Using " << nFeatureRegions << " feature regions per scan." << std::endl + << " - Using +/- " << curvatureRegion << " points for curvature calculation." << std::endl + << " - Using at most " << maxCornerSharp << " sharp" << std::endl + << " and " << maxCornerLessSharp << " less sharp corner points per feature region." << std::endl + << " - Using at most " << maxSurfaceFlat << " flat surface points per feature region." << std::endl + << " - Using " << surfaceCurvatureThreshold << " as surface curvature threshold." << std::endl + << " - Using " << lessFlatFilterSize << " as less flat surface points voxel filter size."); + }; +} RegistrationParams; + + + +/** IMU state data. */ +typedef struct IMUState { + /** The time of the measurement leading to this state (in seconds). */ + ros::Time stamp; + + /** The current roll angle. */ + Angle roll; + + /** The current pitch angle. */ + Angle pitch; + + /** The current yaw angle. */ + Angle yaw; + + /** The accumulated global IMU position in 3D space. */ + Vector3 position; + + /** The accumulated global IMU velocity in 3D space. */ + Vector3 velocity; + + /** The current (local) IMU acceleration in 3D space. */ + Vector3 acceleration; + + /** \brief Interpolate between two IMU states. + * + * @param start the first IMUState + * @param end the second IMUState + * @param ratio the interpolation ratio + * @param result the target IMUState for storing the interpolation result + */ + static void interpolate(const IMUState& start, + const IMUState& end, + const float& ratio, + IMUState& result) + { + float invRatio = 1 - ratio; + + result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio; + result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio; + if (start.yaw.rad() - end.yaw.rad() > M_PI) { + result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio; + } else if (start.yaw.rad() - end.yaw.rad() < -M_PI) { + result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio; + } else { + result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio; + } + + result.velocity = start.velocity * invRatio + end.velocity * ratio; + result.position = start.position * invRatio + end.position * ratio; + }; +} IMUState; + + + +/** \brief Base class for scan registration implementations. + * + * As there exist various sensor devices, producing differently formatted point clouds, + * specific implementations are needed for each group of sensor devices to achieve an accurate registration. + * This class provides common configurations, buffering and processing logic. + */ +class ScanRegistration { +public: + ScanRegistration(const float& scanPeriod, + const uint16_t& nScans = 0, + const size_t& imuHistorySize = 200, + const RegistrationParams& config = RegistrationParams()); + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + virtual bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Setup component in passive mode. + * + * @param config the scan registration configuration parameters + */ + virtual bool setup(const RegistrationParams& config); + + /** \brief Handler method for IMU messages. + * + * @param imuIn the new IMU message + */ + virtual void handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn); + + /** \brief Retrieve the current full resolution input cloud. + * + * @return the current full resolution input cloud + */ + pcl::PointCloud::Ptr getFullResCloud() { return _laserCloud; }; + + /** \brief Retrieve the current sharp corner input sub cloud. + * + * @return the current sharp corner input sub cloud + */ + pcl::PointCloud::Ptr getCornerPointsSharp() { return _cornerPointsSharp; }; + + /** \brief Retrieve the current less sharp corner input sub cloud. + * + * @return the current less sharp corner input sub cloud + */ + pcl::PointCloud::Ptr getCornerPointsLessSharp() { return _cornerPointsLessSharp; }; + + /** \brief Retrieve the current flat surface input sub cloud. + * + * @return the current flat surface input sub cloud + */ + pcl::PointCloud::Ptr getSurfacePointsFlat() { return _surfacePointsFlat; }; + + /** \brief Retrieve the current less flat surface input sub cloud. + * + * @return the current less flat surface input sub cloud + */ + pcl::PointCloud::Ptr getSurfacePointsLessFlat() { return _surfacePointsLessFlat; }; + + /** \brief Retrieve the current IMU transformation information. + * + * @return the current IMU transformation information + */ + pcl::PointCloud::Ptr getIMUTrans() { return _imuTrans; }; + + +protected: + /** \brief Prepare for next sweep: Reset internal cloud buffers and re-initialize start IMU state. + * + * @param scanTime the current scan time + */ + void reset(const ros::Time& scanTime); + + /** \brief Project the given point to the start of the sweep, using the current IMU state and relative time. + * + * @param point the point to project + * @param relTime the relative point measurement time + */ + void transformToStartIMU(pcl::PointXYZI& point, + const float& relTime); + + /** \brief Extract features from current laser cloud. + * + * @param beginIdx the index of the first scan to extract features from + */ + void extractFeatures(const uint16_t& beginIdx = 0); + + /** \brief Set up region buffers for the specified point range. + * + * @param startIdx the region start index + * @param endIdx the region end index + */ + void setRegionBuffersFor(const size_t& startIdx, + const size_t& endIdx); + + /** \brief Set up scan buffers for the specified point range. + * + * @param startIdx the scan start index + * @param endIdx the scan start index + */ + void setScanBuffersFor(const size_t& startIdx, + const size_t& endIdx); + + /** \brief Set sweep end IMU transformation information. + * + * @param sweepDuration the total duration of the current sweep + */ + void setIMUTrans(const double& sweepDuration); + + /** \brief Generate a point cloud message for the specified cloud. */ + void generateROSMsg(sensor_msgs::PointCloud2& msg, + const pcl::PointCloud::Ptr& cloud); + + /** \brief Publish the current result via the respective topics. */ + void publishResult(); + + +protected: + const uint16_t _nScans; ///< number of scans per sweep + const float _scanPeriod; ///< time per scan + ros::Time _sweepStamp; ///< time stamp of the beginning of current sweep + RegistrationParams _config; ///< registration parameter + + IMUState _imuStart; ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan + IMUState _imuCur; ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point + size_t _imuStartIdx; ///< the index in the IMU history of the first IMU state received after the current scan time + CircularBuffer _imuHistory; ///< history of IMU states for cloud registration + + pcl::PointCloud::Ptr _laserCloud; ///< full resolution input cloud + std::vector _scanStartIndices; ///< start indices of the individual scans + std::vector _scanEndIndices; ///< end indices of the individual scans + + pcl::PointCloud::Ptr _cornerPointsSharp; ///< sharp corner points cloud + pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud + pcl::PointCloud::Ptr _surfacePointsFlat; ///< flat surface points cloud + pcl::PointCloud::Ptr _surfacePointsLessFlat; ///< less flat surface points cloud + pcl::PointCloud::Ptr _imuTrans; ///< IMU transformation information + + std::vector _regionCurvature; ///< point curvature buffer + std::vector _regionLabel; ///< point label buffer + std::vector _regionSortIndices; ///< sorted region indices based on point curvature + std::vector _scanNeighborPicked; ///< flag if neighboring point was already picked + + bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) + + ros::Subscriber _subImu; ///< IMU message subscriber + + ros::Publisher _pubLaserCloud; ///< full resolution cloud message publisher + ros::Publisher _pubCornerPointsSharp; ///< sharp corner cloud message publisher + ros::Publisher _pubCornerPointsLessSharp; ///< less sharp corner cloud message publisher + ros::Publisher _pubSurfPointsFlat; ///< flat surface cloud message publisher + ros::Publisher _pubSurfPointsLessFlat; ///< less flat surface cloud message publisher + ros::Publisher _pubImuTrans; ///< IMU transformation message publisher +}; + +} // end namespace loam + + +#endif //LOAM_SCANREGISTRATION_H diff --git a/src/lib/VelodyneScanRegistration.cpp b/src/lib/VelodyneScanRegistration.cpp new file mode 100644 index 0000000..467cd6a --- /dev/null +++ b/src/lib/VelodyneScanRegistration.cpp @@ -0,0 +1,196 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "VelodyneScanRegistration.h" + + +namespace loam { + +VelodyneScanRegistration::VelodyneScanRegistration(const float& scanPeriod, + const uint16_t& nScans, + const size_t& imuHistorySize, + const RegistrationParams& config) + : ScanRegistration(scanPeriod, nScans, imuHistorySize, config), + _systemDelay(20) +{ + +}; + + + +bool VelodyneScanRegistration::setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode) +{ + if (!ScanRegistration::setup(node, privateNode)) { + return false; + } + + _subLaserCloud = node.subscribe + ("/velodyne_points", 2, &VelodyneScanRegistration::processCloudMessage, this); + + return true; +} + + + +void VelodyneScanRegistration::processCloudMessage(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) +{ + if (_systemDelay > 0) { + _systemDelay--; + return; + } + + // fetch new input cloud + pcl::PointCloud laserCloudIn; + pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); + + process(laserCloudIn, laserCloudMsg->header.stamp); +} + + + +void VelodyneScanRegistration::process(const pcl::PointCloud& laserCloudIn, + const ros::Time& scanTime) +{ + size_t cloudSize = laserCloudIn.points.size(); + + // reset internal buffers and set IMU start state based on current scan time + reset(scanTime); + + // determine scan start and end orientations + float startOri = -std::atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); + float endOri = -std::atan2(laserCloudIn.points[cloudSize - 1].y, + laserCloudIn.points[cloudSize - 1].x) + 2 * float(M_PI); + if (endOri - startOri > 3 * M_PI) { + endOri -= 2 * M_PI; + } else if (endOri - startOri < M_PI) { + endOri += 2 * M_PI; + } + + size_t imuIdx = _imuStartIdx; + bool halfPassed = false; + pcl::PointXYZI point; + std::vector > laserCloudScans(_nScans); + + // extract valid points from input cloud + for (int i = 0; i < cloudSize; i++) { + point.x = laserCloudIn.points[i].y; + point.y = laserCloudIn.points[i].z; + point.z = laserCloudIn.points[i].x; + + // skip NaN and INF valued points + if (!pcl_isfinite(point.x) || + !pcl_isfinite(point.y) || + !pcl_isfinite(point.z)) { + continue; + } + + // skip zero valued points + if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) { + continue; + } + + // calculate vertical point angle and scan ID + float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z)) * 180 / float(M_PI); + int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : 0.5)); + int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScans - 1); + if (scanID > (_nScans - 1) || scanID < 0 ){ + continue; + } + + // calculate horizontal point angle + float ori = -std::atan2(point.x, point.z); + if (!halfPassed) { + if (ori < startOri - M_PI / 2) { + ori += 2 * M_PI; + } else if (ori > startOri + M_PI * 3 / 2) { + ori -= 2 * M_PI; + } + + if (ori - startOri > M_PI) { + halfPassed = true; + } + } else { + ori += 2 * M_PI; + + if (ori < endOri - M_PI * 3 / 2) { + ori += 2 * M_PI; + } else if (ori > endOri + M_PI / 2) { + ori -= 2 * M_PI; + } + } + + // calculate relative scan time based on point orientation + float relTime = _scanPeriod * (ori - startOri) / (endOri - startOri); + point.intensity = scanID + relTime; + + // project point to the start of the sweep using corresponding IMU data + if (_imuHistory.size() > 0) { + while (imuIdx < _imuHistory.size() - 1 && (scanTime - _imuHistory[imuIdx].stamp).toSec() + relTime > 0) { + imuIdx++; + } + + if (imuIdx == 0 || (scanTime - _imuHistory[imuIdx].stamp).toSec() + relTime > 0) { + _imuCur = _imuHistory[imuIdx]; + } else { + float ratio = ((_imuHistory[imuIdx].stamp - scanTime).toSec() - relTime) + / (_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp).toSec(); + IMUState::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, _imuCur); + } + + transformToStartIMU(point, relTime); + } + + laserCloudScans[scanID].push_back(point); + } + + // construct sorted full resolution cloud + cloudSize = 0; + for (int i = 0; i < _nScans; i++) { + *_laserCloud += laserCloudScans[i]; + + _scanStartIndices[i] = cloudSize; + cloudSize += laserCloudScans[i].points.size(); + _scanEndIndices[i] = cloudSize - 1; + } + + // extract features + extractFeatures(); + + // set final IMU transformation information + setIMUTrans(_scanPeriod); + + // publish result + publishResult(); +} + +} // end namespace loam diff --git a/src/lib/VelodyneScanRegistration.h b/src/lib/VelodyneScanRegistration.h new file mode 100644 index 0000000..4e4434e --- /dev/null +++ b/src/lib/VelodyneScanRegistration.h @@ -0,0 +1,80 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_VELODYNESCANREGISTRATION_H +#define LOAM_VELODYNESCANREGISTRATION_H + + +#include "ScanRegistration.h" + + +namespace loam { + +class VelodyneScanRegistration : virtual public ScanRegistration { +public: + VelodyneScanRegistration(const float& scanPeriod, + const uint16_t& nScans, + const size_t& imuHistorySize = 200, + const RegistrationParams& config = RegistrationParams()); + + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Process a new input cloud message. + * + * @param laserCloudMsg the new input cloud message to process + */ + void processCloudMessage(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); + + /** \brief Process a new input cloud. + * + * @param laserCloudIn the new input cloud to process + * @param scanTime the scan (message) timestamp + */ + void process(const pcl::PointCloud& laserCloudIn, + const ros::Time& scanTime); + +protected: + int _systemDelay; ///< system startup delay counter + ros::Subscriber _subLaserCloud; ///< input cloud message subscriber +}; + +} // end namespace loam + + +#endif //LOAM_VELODYNESCANREGISTRATION_H diff --git a/src/lib/math_utils.h b/src/lib/math_utils.h new file mode 100644 index 0000000..28e26d8 --- /dev/null +++ b/src/lib/math_utils.h @@ -0,0 +1,399 @@ +#ifndef LOAM_MATH_UTILS_H +#define LOAM_MATH_UTILS_H + +#include +#include + + +namespace loam { + +/** \brief Vector4f decorator for convenient handling. + * + */ +class Vector3 : public Eigen::Vector4f +{ +public: + Vector3(float x, float y, float z) + : Eigen::Vector4f(x, y, z, 0) {} + + Vector3(void) + : Eigen::Vector4f(0, 0, 0, 0) {} + + template + Vector3(const Eigen::MatrixBase& other) + : Eigen::Vector4f(other) {} + + Vector3(const pcl::PointXYZI& p) + : Eigen::Vector4f(p.x, p.y, p.z, 0) {} + + template + Vector3& operator=(const Eigen::MatrixBase & rhs) { + this->Eigen::Vector4f::operator=(rhs); + return *this; + } + + Vector3& operator=(const pcl::PointXYZ& rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + Vector3& operator=(const pcl::PointXYZI& rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + float x() const { return (*this)(0); } + float y() const { return (*this)(1); } + float z() const { return (*this)(2); } + + float& x() { return (*this)(0); } + float& y() { return (*this)(1); } + float& z() { return (*this)(2); } + + // easy conversion + operator pcl::PointXYZI() + { + pcl::PointXYZI dst; + dst.x = x(); + dst.y = y(); + dst.z = z(); + dst.intensity = 0; + return dst; + } +}; + + + +/** \brief Class for holding an angle. + * + * This class provides buffered access to sine and cosine values to the represented angular value. + */ +class Angle{ +public: + Angle() + : _radian(0.0), + _cos(1.0), + _sin(0.0) {} + + Angle(float radValue) + : _radian(radValue), + _cos(std::cos(radValue)), + _sin(std::sin(radValue)) {} + + Angle(const Angle& other) + : _radian(other._radian), + _cos(other._cos), + _sin(other._sin) {} + + void operator =(const Angle& rhs){ + _radian = (rhs._radian); + _cos = (rhs._cos); + _sin = (rhs._sin); + } + + void operator +=( const float& radValue) { *this = ( _radian + radValue) ; } + + void operator +=( const Angle& other) { *this = ( _radian + other._radian ); } + + void operator -=( const float& radValue) { *this = ( _radian - radValue ); } + + void operator -=( const Angle& other) { *this = ( _radian - other._radian ); } + + Angle operator-() const + { + Angle out; + out._radian = -_radian; + out._cos = _cos; + out._sin = -(_sin); + return out; + } + + float rad() const { return _radian; } + + float deg() const { return _radian * 180 / M_PI; } + + float cos() const { return _cos; } + + float sin() const { return _sin; } + +private: + float _radian; ///< angle value in radian + float _cos; ///< cosine of the angle + float _sin; ///< sine of the angle +}; + + + +/** \brief Twist composed by three angles and a three-dimensional position. */ +struct Twist { + Angle rot_x; + Angle rot_y; + Angle rot_z; + Vector3 pos; +}; + + + +/** \brief Convert the given radian angle to degrees. + * + * @param radians The radian angle to convert. + * @return The angle in degrees. + */ +inline double rad2deg(double radians) +{ + return radians * 180.0 / M_PI; +} + + + +/** \brief Convert the given radian angle to degrees. + * + * @param radians The radian angle to convert. + * @return The angle in degrees. + */ +inline float rad2deg(float radians) +{ + return (float) (radians * 180.0 / M_PI); +} + + + +/** \brief Convert the given degree angle to radian. + * + * @param degrees The degree angle to convert. + * @return The radian angle. + */ +inline double deg2rad(double degrees) +{ + return degrees * M_PI / 180.0; +} + + + +/** \brief Convert the given degree angle to radian. + * + * @param degrees The degree angle to convert. + * @return The radian angle. + */ +inline float deg2rad(float degrees) +{ + return (float) (degrees * M_PI / 180.0); +} + + + + +/** \brief Calculate the squared difference of the given two points. + * + * @param a The first point. + * @param b The second point. + * @return The squared difference between point a and b. + */ +inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b) +{ + float diffX = a.x - b.x; + float diffY = a.y - b.y; + float diffZ = a.z - b.z; + + return diffX * diffX + diffY * diffY + diffZ * diffZ; +} + + + +/** \brief Calculate the squared difference of the given two points. + * + * @param a The first point. + * @param b The second point. + * @param wb The weighting factor for the SECOND point. + * @return The squared difference between point a and b. + */ +inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b, const float& wb) +{ + float diffX = a.x - b.x * wb; + float diffY = a.y - b.y * wb; + float diffZ = a.z - b.z * wb; + + return diffX * diffX + diffY * diffY + diffZ * diffZ; +} + + +/** \brief Calculate the absolute distance of the point to the origin. + * + * @param p The point. + * @return The distance to the point. + */ +inline float calcPointDistance(const pcl::PointXYZI& p) +{ + return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z); +} + + + +/** \brief Calculate the squared distance of the point to the origin. + * + * @param p The point. + * @return The squared distance to the point. + */ +inline float calcSquaredPointDistance(const pcl::PointXYZI& p) +{ + return p.x * p.x + p.y * p.y + p.z * p.z; +} + + + +/** \brief Rotate the given vector by the specified angle around the x-axis. + * + * @param v the vector to rotate + * @param ang the rotation angle + */ +inline void rotX(Vector3& v, const Angle& ang) +{ + float y = v.y(); + v.y() = ang.cos() * y - ang.sin() * v.z(); + v.z() = ang.sin() * y + ang.cos() * v.z(); +} + +/** \brief Rotate the given point by the specified angle around the x-axis. + * + * @param p the point to rotate + * @param ang the rotation angle + */ +inline void rotX(pcl::PointXYZI& p, const Angle& ang) +{ + float y = p.y; + p.y = ang.cos() * y - ang.sin() * p.z; + p.z = ang.sin() * y + ang.cos() * p.z; +} + + + +/** \brief Rotate the given vector by the specified angle around the y-axis. + * + * @param v the vector to rotate + * @param ang the rotation angle + */ +inline void rotY(Vector3& v, const Angle& ang) +{ + float x = v.x(); + v.x() = ang.cos() * x + ang.sin() * v.z(); + v.z() = ang.cos() * v.z() - ang.sin() * x; +} + +/** \brief Rotate the given point by the specified angle around the y-axis. + * + * @param p the point to rotate + * @param ang the rotation angle + */ +inline void rotY(pcl::PointXYZI& p, const Angle& ang) +{ + float x = p.x; + p.x = ang.cos() * x + ang.sin() * p.z; + p.z = ang.cos() * p.z - ang.sin() * x; +} + + + +/** \brief Rotate the given vector by the specified angle around the z-axis. + * + * @param v the vector to rotate + * @param ang the rotation angle + */ +inline void rotZ(Vector3& v, const Angle& ang) +{ + float x = v.x(); + v.x() = ang.cos() * x - ang.sin() * v.y(); + v.y() = ang.sin() * x + ang.cos() * v.y(); +} + +/** \brief Rotate the given point by the specified angle around the z-axis. + * + * @param p the point to rotate + * @param ang the rotation angle + */ +inline void rotZ(pcl::PointXYZI& p, const Angle& ang) +{ + float x = p.x; + p.x = ang.cos() * x - ang.sin() * p.y; + p.y = ang.sin() * x + ang.cos() * p.y; +} + + + +/** \brief Rotate the given vector by the specified angles around the z-, x- respectively y-axis. + * + * @param v the vector to rotate + * @param angZ the rotation angle around the z-axis + * @param angX the rotation angle around the x-axis + * @param angY the rotation angle around the y-axis + */ +inline void rotateZXY(Vector3& v, + const Angle& angZ, + const Angle& angX, + const Angle& angY) +{ + rotZ(v, angZ); + rotX(v, angX); + rotY(v, angY); +} + +/** \brief Rotate the given point by the specified angles around the z-, x- respectively y-axis. + * + * @param p the point to rotate + * @param angZ the rotation angle around the z-axis + * @param angX the rotation angle around the x-axis + * @param angY the rotation angle around the y-axis + */ +inline void rotateZXY(pcl::PointXYZI& p, + const Angle& angZ, + const Angle& angX, + const Angle& angY) +{ + rotZ(p, angZ); + rotX(p, angX); + rotY(p, angY); +} + + + +/** \brief Rotate the given vector by the specified angles around the y-, x- respectively z-axis. + * + * @param v the vector to rotate + * @param angY the rotation angle around the y-axis + * @param angX the rotation angle around the x-axis + * @param angZ the rotation angle around the z-axis + */ +inline void rotateYXZ(Vector3& v, + const Angle& angY, + const Angle& angX, + const Angle& angZ) +{ + rotY(v, angY); + rotX(v, angX); + rotZ(v, angZ); +} + +/** \brief Rotate the given point by the specified angles around the y-, x- respectively z-axis. + * + * @param p the point to rotate + * @param angY the rotation angle around the y-axis + * @param angX the rotation angle around the x-axis + * @param angZ the rotation angle around the z-axis + */ +inline void rotateYXZ(pcl::PointXYZI& p, + const Angle& angY, + const Angle& angX, + const Angle& angZ) +{ + rotY(p, angY); + rotX(p, angX); + rotZ(p, angZ); +} + +} // end namespace loam + + +#endif // LOAM_MATH_UTILS_H diff --git a/src/math_utils.h b/src/math_utils.h deleted file mode 100644 index af439b1..0000000 --- a/src/math_utils.h +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef MATH_UTILS_H -#define MATH_UTILS_H - -#include - - -class Vector3 : public Eigen::Vector4f -{ -public: - - Vector3(float x, float y, float z):Eigen::Vector4f(x,y,z,0) {} - - Vector3(void):Eigen::Vector4f(0,0,0,0) {} - - template - Vector3(const Eigen::MatrixBase& other) - : Eigen::Vector4f(other) - { } - - template - Vector3& operator=(const Eigen::MatrixBase & other){ - this->Eigen::Vector4f::operator=(other); - return *this; - } - - Vector3(const pcl::PointXYZI& p):Eigen::Vector4f( p.x, p.y, p.z,0) {} - - Vector3& operator=(const pcl::PointXYZI& point) { - x() = point.x; - y() = point.y; - z() = point.z; - return *this; - } - - float x() const { return (*this)(0); } - float y() const { return (*this)(1); } - float z() const { return (*this)(2); } - - float& x() { return (*this)(0); } - float& y() { return (*this)(1); } - float& z() { return (*this)(2); } -}; - -class Angle{ -public: - Angle(): - _ang(0.0), - _cos(1.0), - _sin(0.0) {} - - Angle(float value): - _ang(value), - _cos(std::cos(value)), - _sin(std::sin(value)) {} - - Angle( const Angle& other ): - _ang( other._ang ), - _cos( other._cos ), - _sin( other._sin ) {} - - void operator =( const Angle& other){ - _ang = ( other._ang ); - _cos = ( other._cos ); - _sin = ( other._sin ); - } - - void operator +=( const float& val) { *this = ( _ang + val) ; } - - void operator +=( const Angle& other) { *this = ( _ang + other._ang ); } - - void operator -=( const float& val) { *this = ( _ang - val ); } - - void operator -=( const Angle& other) { *this = ( _ang - other._ang ); } - - Angle operator-() const - { - Angle out; - out._ang = _ang; - out._cos = _cos; - out._sin = -(_sin); - return out; - } - - float value() const { return _ang; } - - float cos() const { return _cos; } - - float sin() const { return _sin; } - -private: - float _ang, _cos, _sin; -}; - - -inline Vector3 rotateX(const Vector3& v,const Angle& ang) -{ - return Vector3( v.x(), - ang.cos() * v.y() - ang.sin() * v.z(), - ang.sin() * v.y() + ang.cos() * v.z() ); -} - -inline Vector3 rotateY(const Vector3& v,const Angle& ang) -{ - return Vector3( ang.cos() * v.x() + ang.sin() * v.z(), - v.y(), - -ang.sin() * v.x() + ang.cos() * v.z() ); -} - -inline Vector3 rotateZ(const Vector3& v,const Angle& ang) -{ - return Vector3( ang.cos() * v.x() - ang.sin() * v.y(), - ang.sin() * v.x() + ang.cos() * v.y(), - v.z() ); -} - -struct Twist{ - Angle rot_x; - Angle rot_y; - Angle rot_z; - Vector3 pos; -}; - - -#endif // MATH_UTILS_H diff --git a/src/scanRegistration.cpp b/src/scanRegistration.cpp deleted file mode 100755 index 59aed93..0000000 --- a/src/scanRegistration.cpp +++ /dev/null @@ -1,692 +0,0 @@ -// Copyright 2013, Ji Zhang, Carnegie Mellon University -// Further contributions copyright (c) 2016, Southwest Research Institute -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This is an implementation of the algorithm described in the following paper: -// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. -// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "loam_velodyne/common.h" -#include "math_utils.h" - -using std::sin; -using std::cos; -using std::atan2; - -/** Point label options. */ -enum PointLabel { - CORNER_SHARP = 2, ///< sharp corner point - CORNER_LESS_SHARP = 1, ///< less sharp corner point - SURFACE_LESS_FLAT = 0, ///< less flat surface point - SURFACE_FLAT = -1 ///< flat surface point -}; - - -const double scanPeriod = 0.1; - -/** The number of (equally sized) regions used to distribute the feature extraction within a scan. */ -int N_FEATURE_REGIONS = 6; - -/** The number of surrounding points (+/- region around a point) used to calculate a point curvature. */ -int CURVATURE_REGION = 5; - -/** The maximum number of sharp corner points per feature region. */ -int MAX_CORNER_SHARP = 2; - -/** The maximum number of less sharp corner points per feature region. */ -int MAX_CORNER_LESS_SHARP; - -/** The maximum number of flat surface points per feature region. */ -int MAX_SURFACE_FLAT = 4; - -/** The voxel size used for down sizing the remaining less flat surface points. */ -float LESS_FLAT_FILTER_SIZE = 0.2; - -/** The curvature threshold below / above which a point is considered a flat / corner point. */ -float SURFACE_CURVATURE_THRESHOLD = 0.1; - -const int systemDelay = 20; -int systemInitCount = 0; -bool systemInited = false; - -const int N_SCANS = 16; - -float cloudCurvature[40000]; -int cloudSortInd[40000]; -int cloudNeighborPicked[40000]; -PointLabel cloudLabel[40000]; - -int imuPointerFront = 0; -int imuPointerLast = -1; -const int imuQueLength = 200; - -Angle imuRollStart, imuPitchStart, imuYawStart; -Angle imuRollCur, imuPitchCur, imuYawCur; - -Vector3 imuVeloStart; -Vector3 imuShiftStart; -Vector3 imuVeloCur; -Vector3 imuShiftCur; - -Vector3 imuShiftFromStartCur; -Vector3 imuVeloFromStartCur; - -double imuTime[imuQueLength] = {0}; -float imuRoll[imuQueLength] = {0}; -float imuPitch[imuQueLength] = {0}; -float imuYaw[imuQueLength] = {0}; - -Vector3 imuAcc[imuQueLength]; -Vector3 imuVelo[imuQueLength]; -Vector3 imuShift[imuQueLength]; - -ros::Publisher pubLaserCloud; -ros::Publisher pubCornerPointsSharp; -ros::Publisher pubCornerPointsLessSharp; -ros::Publisher pubSurfPointsFlat; -ros::Publisher pubSurfPointsLessFlat; -ros::Publisher pubImuTrans; - -void ShiftToStartIMU(float pointTime) -{ - imuShiftFromStartCur = imuShiftCur - imuShiftStart - imuVeloStart * pointTime; - - Vector3 v1 = rotateY( imuShiftFromStartCur, -imuYawStart); - Vector3 v2 = rotateX( v1, -imuPitchStart); - imuShiftFromStartCur = rotateZ( v2, -imuRollStart); -} - -void VeloToStartIMU() -{ - imuVeloFromStartCur = imuVeloCur - imuVeloStart; - - Vector3 v1 = rotateY( imuVeloFromStartCur, -imuYawStart); - Vector3 v2 = rotateX( v1, -imuPitchStart); - imuVeloFromStartCur = rotateZ( v2, -imuRollStart); -} - -void TransformToStartIMU(PointType *p) -{ - Vector3 v1 = rotateZ( *p, imuRollCur); - Vector3 v2 = rotateX( v1, imuPitchCur); - Vector3 v3 = rotateY( v2, imuYawCur); - - Vector3 v4 = rotateY( v2, -imuYawStart); - Vector3 v5 = rotateX( v1, -imuPitchStart); - Vector3 v6 = rotateZ( *p, -imuRollStart); - - v6 += imuShiftFromStartCur; - - p->x = v6.x(); - p->y = v6.y(); - p->z = v6.z(); -} - -void AccumulateIMUShift() -{ - float roll = imuRoll[imuPointerLast]; - float pitch = imuPitch[imuPointerLast]; - float yaw = imuYaw[imuPointerLast]; - Vector3 acc = imuAcc[imuPointerLast]; - - Vector3 v1 = rotateZ( acc, roll ); - Vector3 v2 = rotateX( v1, pitch ); - acc = rotateY( v2, yaw ); - - - int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength; - double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; - if (timeDiff < scanPeriod) { - - imuShift[imuPointerLast] = imuShift[imuPointerBack] + (imuVelo[imuPointerBack] * timeDiff) - + acc * (0.5* timeDiff * timeDiff); - - imuVelo[imuPointerLast] = imuVelo[imuPointerBack] + acc * timeDiff; - } -} - -void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) -{ - if (!systemInited) { - systemInitCount++; - if (systemInitCount >= systemDelay) { - systemInited = true; - } - return; - } - - std::vector scanStartInd(N_SCANS, 0); - std::vector scanEndInd(N_SCANS, 0); - - double timeScanCur = laserCloudMsg->header.stamp.toSec(); - pcl::PointCloud laserCloudIn; - pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); - std::vector indices; - pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); - int cloudSize = laserCloudIn.points.size(); - float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); - float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, - laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; - - if (endOri - startOri > 3 * M_PI) { - endOri -= 2 * M_PI; - } else if (endOri - startOri < M_PI) { - endOri += 2 * M_PI; - } - bool halfPassed = false; - int count = cloudSize; - PointType point; - std::vector > laserCloudScans(N_SCANS); - for (int i = 0; i < cloudSize; i++) { - point.x = laserCloudIn.points[i].y; - point.y = laserCloudIn.points[i].z; - point.z = laserCloudIn.points[i].x; - - float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI; - int scanID; - int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); - if (roundedAngle > 0){ - scanID = roundedAngle; - } - else { - scanID = roundedAngle + (N_SCANS - 1); - } - if (scanID > (N_SCANS - 1) || scanID < 0 ){ - count--; - continue; - } - - float ori = -atan2(point.x, point.z); - if (!halfPassed) { - if (ori < startOri - M_PI / 2) { - ori += 2 * M_PI; - } else if (ori > startOri + M_PI * 3 / 2) { - ori -= 2 * M_PI; - } - - if (ori - startOri > M_PI) { - halfPassed = true; - } - } else { - ori += 2 * M_PI; - - if (ori < endOri - M_PI * 3 / 2) { - ori += 2 * M_PI; - } else if (ori > endOri + M_PI / 2) { - ori -= 2 * M_PI; - } - } - - float relTime = (ori - startOri) / (endOri - startOri); - point.intensity = scanID + scanPeriod * relTime; - - if (imuPointerLast >= 0) { - float pointTime = relTime * scanPeriod; - while (imuPointerFront != imuPointerLast) { - if (timeScanCur + pointTime < imuTime[imuPointerFront]) { - break; - } - imuPointerFront = (imuPointerFront + 1) % imuQueLength; - } - - if (timeScanCur + pointTime > imuTime[imuPointerFront]) { - imuRollCur = imuRoll[imuPointerFront]; - imuPitchCur = imuPitch[imuPointerFront]; - imuYawCur = imuYaw[imuPointerFront]; - - imuVeloCur = imuVelo[imuPointerFront]; - imuShiftCur = imuShift[imuPointerFront]; - } else { - int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; - float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) - / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); - float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) - / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); - - imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; - imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; - if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) { - imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack; - } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) { - imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack; - } else { - imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; - } - - imuVeloCur = imuVelo[imuPointerFront] * ratioFront + imuVelo[imuPointerBack] * ratioBack; - imuShiftCur = imuShift[imuPointerFront] * ratioFront + imuShift[imuPointerBack] * ratioBack; - } - if (i == 0) { - imuRollStart = imuRollCur; - imuPitchStart = imuPitchCur; - imuYawStart = imuYawCur; - - imuVeloStart = imuVeloCur; - imuShiftStart = imuShiftCur; - - } else { - ShiftToStartIMU(pointTime); - VeloToStartIMU(); - TransformToStartIMU(&point); - } - } - laserCloudScans[scanID].push_back(point); - } - cloudSize = count; - - pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); - for (int i = 0; i < N_SCANS; i++) { - *laserCloud += laserCloudScans[i]; - } - - int scanCount = -1; - float pointWeight = -2 * CURVATURE_REGION; - for (int i = CURVATURE_REGION; i < cloudSize - CURVATURE_REGION; i++) { - float diffX = pointWeight * laserCloud->points[i].x; - float diffY = pointWeight * laserCloud->points[i].y; - float diffZ = pointWeight * laserCloud->points[i].z; - - for (int j = 1; j <= CURVATURE_REGION; j++) { - diffX += laserCloud->points[i + j].x + laserCloud->points[i - j].x; - diffY += laserCloud->points[i + j].y + laserCloud->points[i - j].y; - diffZ += laserCloud->points[i + j].z + laserCloud->points[i - j].z; - } - - cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; - cloudSortInd[i] = i; - cloudNeighborPicked[i] = 0; - cloudLabel[i] = SURFACE_LESS_FLAT; - - if (int(laserCloud->points[i].intensity) != scanCount) { - scanCount = int(laserCloud->points[i].intensity); - - if (scanCount > 0 && scanCount < N_SCANS) { - scanStartInd[scanCount] = i + CURVATURE_REGION; - scanEndInd[scanCount - 1] = i - CURVATURE_REGION; - } - } - } - scanStartInd[0] = CURVATURE_REGION; - scanEndInd.back() = cloudSize - CURVATURE_REGION; - - for (int i = CURVATURE_REGION; i < cloudSize - CURVATURE_REGION - 1; i++) { - float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x; - float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y; - float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z; - float diff = diffX * diffX + diffY * diffY + diffZ * diffZ; - - if (diff > 0.1) { - - float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + - laserCloud->points[i].y * laserCloud->points[i].y + - laserCloud->points[i].z * laserCloud->points[i].z); - - float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + - laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + - laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); - - if (depth1 > depth2) { - diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1; - diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1; - diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1; - - if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) { - for (int j = 0; j <= CURVATURE_REGION; j++) { - cloudNeighborPicked[i - j] = 1; - } - } - } else { - diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x; - diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y; - diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z; - - if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) { - for (int j = CURVATURE_REGION + 1; j > 0 ; j--) { - cloudNeighborPicked[i + j] = 1; - } - } - } - } - - float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x; - float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y; - float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z; - float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2; - - float dis = laserCloud->points[i].x * laserCloud->points[i].x - + laserCloud->points[i].y * laserCloud->points[i].y - + laserCloud->points[i].z * laserCloud->points[i].z; - - if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { - cloudNeighborPicked[i] = 1; - } - } - - - pcl::PointCloud cornerPointsSharp; - pcl::PointCloud cornerPointsLessSharp; - pcl::PointCloud surfPointsFlat; - pcl::PointCloud surfPointsLessFlat; - - for (int i = 0; i < N_SCANS; i++) { - pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); - for (int j = 0; j < N_FEATURE_REGIONS; j++) { - int sp = (scanStartInd[i] * (N_FEATURE_REGIONS - j) + scanEndInd[i] * j) / N_FEATURE_REGIONS; - int ep = (scanStartInd[i] * (N_FEATURE_REGIONS - 1 - j) + scanEndInd[i] * (j + 1)) / N_FEATURE_REGIONS - 1; - - for (int k = sp + 1; k <= ep; k++) { - for (int l = k; l >= sp + 1; l--) { - if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) { - int temp = cloudSortInd[l - 1]; - cloudSortInd[l - 1] = cloudSortInd[l]; - cloudSortInd[l] = temp; - } - } - } - - int largestPickedNum = 0; - for (int k = ep; k >= sp; k--) { - int ind = cloudSortInd[k]; - if (cloudNeighborPicked[ind] == 0 && - cloudCurvature[ind] > SURFACE_CURVATURE_THRESHOLD) { - - largestPickedNum++; - if (largestPickedNum <= MAX_CORNER_SHARP) { - cloudLabel[ind] = CORNER_SHARP; - cornerPointsSharp.push_back(laserCloud->points[ind]); - cornerPointsLessSharp.push_back(laserCloud->points[ind]); - } else if (largestPickedNum <= MAX_CORNER_LESS_SHARP) { - cloudLabel[ind] = CORNER_LESS_SHARP; - cornerPointsLessSharp.push_back(laserCloud->points[ind]); - } else { - break; - } - - cloudNeighborPicked[ind] = 1; - for (int l = 1; l <= CURVATURE_REGION; l++) { - float diffX = laserCloud->points[ind + l].x - - laserCloud->points[ind + l - 1].x; - float diffY = laserCloud->points[ind + l].y - - laserCloud->points[ind + l - 1].y; - float diffZ = laserCloud->points[ind + l].z - - laserCloud->points[ind + l - 1].z; - if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { - break; - } - - cloudNeighborPicked[ind + l] = 1; - } - for (int l = -1; l >= -CURVATURE_REGION; l--) { - float diffX = laserCloud->points[ind + l].x - - laserCloud->points[ind + l + 1].x; - float diffY = laserCloud->points[ind + l].y - - laserCloud->points[ind + l + 1].y; - float diffZ = laserCloud->points[ind + l].z - - laserCloud->points[ind + l + 1].z; - if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { - break; - } - - cloudNeighborPicked[ind + l] = 1; - } - } - } - - int smallestPickedNum = 0; - for (int k = sp; k <= ep; k++) { - int ind = cloudSortInd[k]; - if (cloudNeighborPicked[ind] == 0 && - cloudCurvature[ind] < SURFACE_CURVATURE_THRESHOLD) { - - cloudLabel[ind] = SURFACE_FLAT; - surfPointsFlat.push_back(laserCloud->points[ind]); - - smallestPickedNum++; - if (smallestPickedNum >= MAX_SURFACE_FLAT) { - break; - } - - cloudNeighborPicked[ind] = 1; - for (int l = 1; l <= CURVATURE_REGION; l++) { - float diffX = laserCloud->points[ind + l].x - - laserCloud->points[ind + l - 1].x; - float diffY = laserCloud->points[ind + l].y - - laserCloud->points[ind + l - 1].y; - float diffZ = laserCloud->points[ind + l].z - - laserCloud->points[ind + l - 1].z; - if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { - break; - } - - cloudNeighborPicked[ind + l] = 1; - } - for (int l = -1; l >= -CURVATURE_REGION; l--) { - float diffX = laserCloud->points[ind + l].x - - laserCloud->points[ind + l + 1].x; - float diffY = laserCloud->points[ind + l].y - - laserCloud->points[ind + l + 1].y; - float diffZ = laserCloud->points[ind + l].z - - laserCloud->points[ind + l + 1].z; - if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { - break; - } - - cloudNeighborPicked[ind + l] = 1; - } - } - } - - for (int k = sp; k <= ep; k++) { - if (cloudLabel[k] <= SURFACE_LESS_FLAT) { - surfPointsLessFlatScan->push_back(laserCloud->points[k]); - } - } - } - - pcl::PointCloud surfPointsLessFlatScanDS; - pcl::VoxelGrid downSizeFilter; - downSizeFilter.setInputCloud(surfPointsLessFlatScan); - downSizeFilter.setLeafSize(LESS_FLAT_FILTER_SIZE, LESS_FLAT_FILTER_SIZE, LESS_FLAT_FILTER_SIZE); - downSizeFilter.filter(surfPointsLessFlatScanDS); - - surfPointsLessFlat += surfPointsLessFlatScanDS; - } - - sensor_msgs::PointCloud2 laserCloudOutMsg; - pcl::toROSMsg(*laserCloud, laserCloudOutMsg); - laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; - laserCloudOutMsg.header.frame_id = "/camera"; - pubLaserCloud.publish(laserCloudOutMsg); - - sensor_msgs::PointCloud2 cornerPointsSharpMsg; - pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); - cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; - cornerPointsSharpMsg.header.frame_id = "/camera"; - pubCornerPointsSharp.publish(cornerPointsSharpMsg); - - sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; - pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); - cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; - cornerPointsLessSharpMsg.header.frame_id = "/camera"; - pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); - - sensor_msgs::PointCloud2 surfPointsFlat2; - pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); - surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; - surfPointsFlat2.header.frame_id = "/camera"; - pubSurfPointsFlat.publish(surfPointsFlat2); - - sensor_msgs::PointCloud2 surfPointsLessFlat2; - pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); - surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; - surfPointsLessFlat2.header.frame_id = "/camera"; - pubSurfPointsLessFlat.publish(surfPointsLessFlat2); - - pcl::PointCloud imuTrans(4, 1); - imuTrans.points[0].x = imuPitchStart.value(); - imuTrans.points[0].y = imuYawStart.value(); - imuTrans.points[0].z = imuRollStart.value(); - - imuTrans.points[1].x = imuPitchCur.value(); - imuTrans.points[1].y = imuYawCur.value(); - imuTrans.points[1].z = imuRollCur.value(); - - imuTrans.points[2].x = imuShiftFromStartCur.x(); - imuTrans.points[2].y = imuShiftFromStartCur.y(); - imuTrans.points[2].z = imuShiftFromStartCur.z(); - - imuTrans.points[3].x = imuVeloFromStartCur.x(); - imuTrans.points[3].y = imuVeloFromStartCur.y(); - imuTrans.points[3].z = imuVeloFromStartCur.z(); - - sensor_msgs::PointCloud2 imuTransMsg; - pcl::toROSMsg(imuTrans, imuTransMsg); - imuTransMsg.header.stamp = laserCloudMsg->header.stamp; - imuTransMsg.header.frame_id = "/camera"; - pubImuTrans.publish(imuTransMsg); -} - -void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) -{ - double roll, pitch, yaw; - tf::Quaternion orientation; - tf::quaternionMsgToTF(imuIn->orientation, orientation); - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - - Vector3 acc; - acc.x() = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81; - acc.y() = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; - acc.z() = imuIn->linear_acceleration.x + sin(pitch) * 9.81; - - imuPointerLast = (imuPointerLast + 1) % imuQueLength; - - imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); - imuRoll[imuPointerLast] = roll; - imuPitch[imuPointerLast] = pitch; - imuYaw[imuPointerLast] = yaw; - imuAcc[imuPointerLast] = acc; - - AccumulateIMUShift(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "scanRegistration"); - ros::NodeHandle nh; - - - N_FEATURE_REGIONS = nh.param("/scanRegistration/featureRegions", N_FEATURE_REGIONS); - if (N_FEATURE_REGIONS < 1) { - ROS_FATAL("Invalid featureRegions parameter: %d (expected >= 1)", N_FEATURE_REGIONS); - ros::shutdown(); - } - - CURVATURE_REGION = nh.param("/scanRegistration/curvatureRegion", CURVATURE_REGION); - if (CURVATURE_REGION < 1) { - ROS_FATAL("Invalid curvatureRegion parameter: %d (expected >= 1)", CURVATURE_REGION); - ros::shutdown(); - } - - MAX_CORNER_SHARP = nh.param("/scanRegistration/maxCornerSharp", MAX_CORNER_SHARP); - if (MAX_CORNER_SHARP < 1) { - ROS_FATAL("Invalid maxCornerSharp parameter: %d (expected >= 1)", MAX_CORNER_SHARP); - ros::shutdown(); - } - - MAX_CORNER_LESS_SHARP = nh.param("/scanRegistration/maxCornerLessSharp", 10 * MAX_CORNER_SHARP); - if (MAX_CORNER_LESS_SHARP < MAX_CORNER_SHARP) { - ROS_FATAL("Invalid maxCornerLessSharp parameter: %d (expected >= %d)", MAX_CORNER_LESS_SHARP, MAX_CORNER_SHARP); - ros::shutdown(); - } - - MAX_SURFACE_FLAT = nh.param("/scanRegistration/maxSurfaceFlat", MAX_SURFACE_FLAT); - if (MAX_SURFACE_FLAT < 1) { - ROS_FATAL("Invalid maxSurfaceFlat parameter: %d (expected >= 1)", MAX_SURFACE_FLAT); - ros::shutdown(); - } - - SURFACE_CURVATURE_THRESHOLD = nh.param("/scanRegistration/surfaceCurvatureThreshold", SURFACE_CURVATURE_THRESHOLD); - if (SURFACE_CURVATURE_THRESHOLD < 0.001) { - ROS_FATAL("Invalid surfaceCurvatureThreshold parameter: %f (expected >= 0.001)", SURFACE_CURVATURE_THRESHOLD); - ros::shutdown(); - } - - LESS_FLAT_FILTER_SIZE = nh.param("/scanRegistration/lessFlatFilterSize", LESS_FLAT_FILTER_SIZE); - if (LESS_FLAT_FILTER_SIZE < 0.001) { - ROS_FATAL("Invalid lessFlatFilterSize parameter: %f (expected >= 0.001)", LESS_FLAT_FILTER_SIZE); - ros::shutdown(); - } - - - ROS_INFO("Using %d feature regions per scan.", N_FEATURE_REGIONS); - ROS_INFO("Using +/- %d points for curvature calculation.", CURVATURE_REGION); - ROS_INFO("Using at most %d sharp and %d less sharp corner points per feature region.", - MAX_CORNER_SHARP, MAX_CORNER_LESS_SHARP); - ROS_INFO("Using at most %d flat surface points per feature region.", MAX_SURFACE_FLAT); - ROS_INFO("Using %g as surface curvature threshold.", SURFACE_CURVATURE_THRESHOLD); - ROS_INFO("Using %g as less flat surface points voxel filter size.", LESS_FLAT_FILTER_SIZE); - - - ros::Subscriber subLaserCloud = nh.subscribe - ("/velodyne_points", 2, laserCloudHandler); - - ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); - - pubLaserCloud = nh.advertise - ("/velodyne_cloud_2", 2); - - pubCornerPointsSharp = nh.advertise - ("/laser_cloud_sharp", 2); - - pubCornerPointsLessSharp = nh.advertise - ("/laser_cloud_less_sharp", 2); - - pubSurfPointsFlat = nh.advertise - ("/laser_cloud_flat", 2); - - pubSurfPointsLessFlat = nh.advertise - ("/laser_cloud_less_flat", 2); - - pubImuTrans = nh.advertise ("/imu_trans", 5); - - ros::spin(); - - return 0; -} - diff --git a/src/velodyne_scan_registration_node.cpp b/src/velodyne_scan_registration_node.cpp new file mode 100644 index 0000000..3615ae5 --- /dev/null +++ b/src/velodyne_scan_registration_node.cpp @@ -0,0 +1,20 @@ +#include +#include "lib/VelodyneScanRegistration.h" + + +/** Main node entry point. */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "scanRegistration"); + ros::NodeHandle node; + ros::NodeHandle privateNode("~"); + + loam::VelodyneScanRegistration veloScan(0.1, 16); + + if (veloScan.setup(node, privateNode)) { + // initialization successful + ros::spin(); + } + + return 0; +} From 5fe0300f5de78b97949675db590714d8eca2c8c3 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Fri, 1 Dec 2017 18:02:23 +0100 Subject: [PATCH 2/9] Extracted laser odometry component into own class. * Let transformToEnd transform a whole cloud at once instead of only one point, since it was only called in loops over whole clouds. * Reduced scopes of variables where possible. Changes to the logic: * Do not publish corner and surface clouds during initialization, as they are anyhow rejected without the corresponding full resolution cloud (which was not sent during initialization). New Features: * All buffers used during optimization are now dynamic and automatically adapted to the currently processed cloud sizes. * Again, there are two modis: active and passive. However, passive mode requires some further work with respect to result management. --- CMakeLists.txt | 4 +- src/laserOdometry.cpp | 856 ---------------------------- src/laser_odometry_node.cpp | 20 + src/lib/CMakeLists.txt | 4 +- src/lib/LaserOdometry.cpp | 884 +++++++++++++++++++++++++++++ src/lib/LaserOdometry.h | 217 +++++++ src/lib/ScanRegistration.h | 2 +- src/lib/VelodyneScanRegistration.h | 3 + 8 files changed, 1130 insertions(+), 860 deletions(-) delete mode 100755 src/laserOdometry.cpp create mode 100644 src/laser_odometry_node.cpp create mode 100644 src/lib/LaserOdometry.cpp create mode 100644 src/lib/LaserOdometry.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ce5e68c..c65a51b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,8 +34,8 @@ add_subdirectory(src/lib) add_executable(scanRegistration src/velodyne_scan_registration_node.cpp) target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) -add_executable(laserOdometry src/laserOdometry.cpp) -target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) +add_executable(laserOdometry src/laser_odometry_node.cpp) +target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) add_executable(laserMapping src/laserMapping.cpp) target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp deleted file mode 100755 index e1282a2..0000000 --- a/src/laserOdometry.cpp +++ /dev/null @@ -1,856 +0,0 @@ -// Copyright 2013, Ji Zhang, Carnegie Mellon University -// Further contributions copyright (c) 2016, Southwest Research Institute -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This is an implementation of the algorithm described in the following paper: -// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. -// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. - - -#include "loam_velodyne/nanoflann_pcl.h" -#include "lib/math_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace loam; - -const float scanPeriod = 0.1; - -const int skipFrameNum = 1; -bool systemInited = false; - -double timeCornerPointsSharp = 0; -double timeCornerPointsLessSharp = 0; -double timeSurfPointsFlat = 0; -double timeSurfPointsLessFlat = 0; -double timeLaserCloudFullRes = 0; -double timeImuTrans = 0; - -bool newCornerPointsSharp = false; -bool newCornerPointsLessSharp = false; -bool newSurfPointsFlat = false; -bool newSurfPointsLessFlat = false; -bool newLaserCloudFullRes = false; -bool newImuTrans = false; - -pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); -pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); -pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); -pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudOri(new pcl::PointCloud()); -pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); -pcl::PointCloud::Ptr imuTrans(new pcl::PointCloud()); - -nanoflann::KdTreeFLANN kdtreeCornerLast; -nanoflann::KdTreeFLANN kdtreeSurfLast; - -int laserCloudCornerLastNum; -int laserCloudSurfLastNum; - -int pointSelCornerInd[40000]; -float pointSearchCornerInd1[40000]; -float pointSearchCornerInd2[40000]; - -int pointSelSurfInd[40000]; -float pointSearchSurfInd1[40000]; -float pointSearchSurfInd2[40000]; -float pointSearchSurfInd3[40000]; - -Twist transform; -Twist transformSum; - -Angle imuRollStart, imuPitchStart, imuYawStart; -Angle imuRollLast, imuPitchLast, imuYawLast; - -Vector3 imuShiftFromStart; -Vector3 imuVeloFromStart; - -void TransformToStart(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); - - po->x = pi->x - s * transform.pos.x(); - po->y = pi->y - s * transform.pos.y(); - po->z = pi->z - s * transform.pos.z(); - po->intensity = pi->intensity; - - Angle rx = s * transform.rot_x.rad(); - Angle ry = s * transform.rot_y.rad(); - Angle rz = s * transform.rot_z.rad(); - rotateZXY(*po, -rz, -rx, -ry); -} - -void TransformToEnd(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); - - po->x = pi->x - s * transform.pos.x(); - po->y = pi->y - s * transform.pos.y(); - po->z = pi->z - s * transform.pos.z(); - po->intensity = int(pi->intensity); - - Angle rx = s * transform.rot_x.rad(); - Angle ry = s * transform.rot_y.rad(); - Angle rz = s * transform.rot_z.rad(); - rotateZXY(*po, -rz, -rx, -ry); - rotateYXZ(*po, transform.rot_y, transform.rot_x, transform.rot_z); - - po->x += transform.pos.x() - imuShiftFromStart.x(); - po->y += transform.pos.y() - imuShiftFromStart.y(); - po->z += transform.pos.z() - imuShiftFromStart.z(); - - rotateZXY(*po, imuRollStart, imuPitchStart, imuYawStart); - rotateYXZ(*po, -imuYawLast, -imuPitchLast, -imuRollLast); -} - -void PluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, - const Angle& blx, const Angle& bly, const Angle& blz, - const Angle& alx, const Angle& aly, const Angle& alz, - Angle &acx, Angle &acy, Angle &acz) -{ - float sbcx = bcx.sin(); - float cbcx = bcx.cos(); - float sbcy = bcy.sin(); - float cbcy = bcy.cos(); - float sbcz = bcz.sin(); - float cbcz = bcz.cos(); - - float sblx = blx.sin(); - float cblx = blx.cos(); - float sbly = bly.sin(); - float cbly = bly.cos(); - float sblz = blz.sin(); - float cblz = blz.cos(); - - float salx = alx.sin(); - float calx = alx.cos(); - float saly = aly.sin(); - float caly = aly.cos(); - float salz = alz.sin(); - float calz = alz.cos(); - - float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); - acx = -asin(srx); - - float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); - float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); - acy = atan2(srycrx / acx.cos(), crycrx / acx.cos()); - - float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) - + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz - + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) - + calx*cblx*salz*sblz); - float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) - + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) - + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) - + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - - calx*calz*cblx*sblz); - acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos()); -} - -void AccumulateRotation(Angle cx, Angle cy, Angle cz, - Angle lx, Angle ly, Angle lz, - Angle &ox, Angle &oy, Angle &oz) -{ - float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin() - cx.cos()*cz.cos()*lx.sin() - lx.cos()*ly.cos()*cx.sin(); - ox = -asin(srx); - - float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin()) + lx.cos()*ly.sin()*(cy.cos()*cz.cos() - + cx.sin()*cy.sin()*cz.sin()) + lx.cos()*ly.cos()*cx.cos()*cy.sin(); - float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos() - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - - cy.cos()*cx.sin()*cz.sin()) - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin()); - oy = atan2(srycrx / ox.cos(), crycrx / ox.cos()); - - float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin()) + cx.cos()*cz.sin()*(ly.cos()*lz.cos() - + lx.sin()*ly.sin()*lz.sin()) + lx.cos()*cx.cos()*cz.cos()*lz.sin(); - float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos() - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - - lz.cos()*lx.sin()*ly.sin()) - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin()); - oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos()); -} - -void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2) -{ - timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec(); - - cornerPointsSharp->clear(); - pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp); - std::vector indices; - pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); - newCornerPointsSharp = true; -} - -void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2) -{ - timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec(); - - cornerPointsLessSharp->clear(); - pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp); - std::vector indices; - pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices); - newCornerPointsLessSharp = true; -} - -void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2) -{ - timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec(); - - surfPointsFlat->clear(); - pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat); - std::vector indices; - pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices); - newSurfPointsFlat = true; -} - -void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2) -{ - timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec(); - - surfPointsLessFlat->clear(); - pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat); - std::vector indices; - pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices); - newSurfPointsLessFlat = true; -} - -void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) -{ - timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); - - laserCloudFullRes->clear(); - pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); - std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices); - newLaserCloudFullRes = true; -} - -void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) -{ - timeImuTrans = imuTrans2->header.stamp.toSec(); - - imuTrans->clear(); - pcl::fromROSMsg(*imuTrans2, *imuTrans); - - imuPitchStart = imuTrans->points[0].x; - imuYawStart = imuTrans->points[0].y; - imuRollStart = imuTrans->points[0].z; - - imuPitchLast = imuTrans->points[1].x; - imuYawLast = imuTrans->points[1].y; - imuRollLast = imuTrans->points[1].z; - - imuShiftFromStart = imuTrans->points[2]; - imuVeloFromStart = imuTrans->points[3]; - - newImuTrans = true; -} - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "laserOdometry"); - ros::NodeHandle nh; - - ros::Subscriber subCornerPointsSharp = nh.subscribe - ("/laser_cloud_sharp", 2, laserCloudSharpHandler); - - ros::Subscriber subCornerPointsLessSharp = nh.subscribe - ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler); - - ros::Subscriber subSurfPointsFlat = nh.subscribe - ("/laser_cloud_flat", 2, laserCloudFlatHandler); - - ros::Subscriber subSurfPointsLessFlat = nh.subscribe - ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler); - - ros::Subscriber subLaserCloudFullRes = nh.subscribe - ("/velodyne_cloud_2", 2, laserCloudFullResHandler); - - ros::Subscriber subImuTrans = nh.subscribe - ("/imu_trans", 5, imuTransHandler); - - ros::Publisher pubLaserCloudCornerLast = nh.advertise - ("/laser_cloud_corner_last", 2); - - ros::Publisher pubLaserCloudSurfLast = nh.advertise - ("/laser_cloud_surf_last", 2); - - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/velodyne_cloud_3", 2); - - ros::Publisher pubLaserOdometry = nh.advertise ("/laser_odom_to_init", 5); - nav_msgs::Odometry laserOdometry; - laserOdometry.header.frame_id = "/camera_init"; - laserOdometry.child_frame_id = "/laser_odom"; - - tf::TransformBroadcaster tfBroadcaster; - tf::StampedTransform laserOdometryTrans; - laserOdometryTrans.frame_id_ = "/camera_init"; - laserOdometryTrans.child_frame_id_ = "/laser_odom"; - - PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff; - - bool isDegenerate = false; - Eigen::Matrix matP; - - int frameCount = skipFrameNum; - ros::Rate rate(100); - bool status = ros::ok(); - while (status) { - ros::spinOnce(); - - if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && - newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans && - fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 && - fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 && - fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 && - fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 && - fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) { - newCornerPointsSharp = false; - newCornerPointsLessSharp = false; - newSurfPointsFlat = false; - newSurfPointsLessFlat = false; - newLaserCloudFullRes = false; - newImuTrans = false; - - if (!systemInited) { - std::swap(cornerPointsLessSharp, laserCloudCornerLast); - std::swap(surfPointsLessFlat, laserCloudSurfLast); - - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudCornerLast2.header.frame_id = "/camera"; - pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudSurfLast2.header.frame_id = "/camera"; - pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - transformSum.rot_x += imuPitchStart; - transformSum.rot_z += imuRollStart; - - kdtreeCornerLast.setInputCloud(laserCloudCornerLast); - kdtreeSurfLast.setInputCloud(laserCloudSurfLast); - - systemInited = true; - continue; - } - - transform.pos -= imuVeloFromStart * scanPeriod; - - if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { - - std::vector pointSearchInd(1); - std::vector pointSearchSqDis(1); - std::vector indices; - - pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); - int cornerPointsSharpNum = cornerPointsSharp->points.size(); - int surfPointsFlatNum = surfPointsFlat->points.size(); - - for (int iterCount = 0; iterCount < 25; iterCount++) { - laserCloudOri->clear(); - coeffSel->clear(); - - for (int i = 0; i < cornerPointsSharpNum; i++) { - TransformToStart(&cornerPointsSharp->points[i], &pointSel); - - if (iterCount % 5 == 0) { - std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices); - kdtreeCornerLast.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); - - int closestPointInd = -1, minPointInd2 = -1; - if (pointSearchSqDis[0] < 25) { - closestPointInd = pointSearchInd[0]; - int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity); - - float pointSqDis, minPointSqDis2 = 25; - for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { - if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); - - if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } - } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); - - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } - } - } - - pointSearchCornerInd1[i] = closestPointInd; - pointSearchCornerInd2[i] = minPointInd2; - } - - if (pointSearchCornerInd2[i] >= 0) { - tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]]; - tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]]; - - float x0 = pointSel.x; - float y0 = pointSel.y; - float z0 = pointSel.z; - float x1 = tripod1.x; - float y1 = tripod1.y; - float z1 = tripod1.z; - float x2 = tripod2.x; - float y2 = tripod2.y; - float z2 = tripod2.z; - - float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) - * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); - - float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); - - float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; - - float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float ld2 = a012 / l12; - - pointProj = pointSel; - pointProj.x -= la * ld2; - pointProj.y -= lb * ld2; - pointProj.z -= lc * ld2; - - float s = 1; - if (iterCount >= 5) { - s = 1 - 1.8 * fabs(ld2); - } - - coeff.x = s * la; - coeff.y = s * lb; - coeff.z = s * lc; - coeff.intensity = s * ld2; - - if (s > 0.1 && ld2 != 0) { - laserCloudOri->push_back(cornerPointsSharp->points[i]); - coeffSel->push_back(coeff); - } - } - } - - for (int i = 0; i < surfPointsFlatNum; i++) { - TransformToStart(&surfPointsFlat->points[i], &pointSel); - - if (iterCount % 5 == 0) { - kdtreeSurfLast.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); - int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; - if (pointSearchSqDis[0] < 25) { - closestPointInd = pointSearchInd[0]; - int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity); - - float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; - for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { - if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); - - if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } else { - if (pointSqDis < minPointSqDis3) { - minPointSqDis3 = pointSqDis; - minPointInd3 = j; - } - } - } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); - - if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } else { - if (pointSqDis < minPointSqDis3) { - minPointSqDis3 = pointSqDis; - minPointInd3 = j; - } - } - } - } - - pointSearchSurfInd1[i] = closestPointInd; - pointSearchSurfInd2[i] = minPointInd2; - pointSearchSurfInd3[i] = minPointInd3; - } - - if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { - tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; - tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; - tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; - - float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); - float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); - float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); - float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); - - float ps = sqrt(pa * pa + pb * pb + pc * pc); - pa /= ps; - pb /= ps; - pc /= ps; - pd /= ps; - - float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; - - pointProj = pointSel; - pointProj.x -= pa * pd2; - pointProj.y -= pb * pd2; - pointProj.z -= pc * pd2; - - float s = 1; - if (iterCount >= 5) { - s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x - + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); - } - - coeff.x = s * pa; - coeff.y = s * pb; - coeff.z = s * pc; - coeff.intensity = s * pd2; - - if (s > 0.1 && pd2 != 0) { - laserCloudOri->push_back(surfPointsFlat->points[i]); - coeffSel->push_back(coeff); - } - } - } - - int pointSelNum = laserCloudOri->points.size(); - if (pointSelNum < 10) { - continue; - } - - Eigen::Matrix matA(pointSelNum, 6); - Eigen::Matrix matAt(6,pointSelNum); - Eigen::Matrix matAtA; - Eigen::VectorXf matB(pointSelNum); - Eigen::Matrix matAtB; - Eigen::Matrix matX; - - for (int i = 0; i < pointSelNum; i++) { - pointOri = laserCloudOri->points[i]; - coeff = coeffSel->points[i]; - - float s = 1; - - float srx = sin(s * transform.rot_x.rad()); - float crx = cos(s * transform.rot_x.rad()); - float sry = sin(s * transform.rot_y.rad()); - float cry = cos(s * transform.rot_y.rad()); - float srz = sin(s * transform.rot_z.rad()); - float crz = cos(s * transform.rot_z.rad()); - float tx = s * transform.pos.x(); - float ty = s * transform.pos.y(); - float tz = s * transform.pos.z(); - - float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z - + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x - + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z - + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y - + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z - + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z; - - float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x - + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z - + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) - + s*tz*crx*cry) * coeff.x - + ((s*cry*crz - s*srx*sry*srz)*pointOri.x - + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z - + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) - - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z; - - float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y - + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x - + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y - + s*ty*crx*srz + s*tx*crx*crz) * coeff.y - + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y - + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z; - - float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y - - s*(crz*sry + cry*srx*srz) * coeff.z; - - float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y - - s*(sry*srz - cry*crz*srx) * coeff.z; - - float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z; - - float d2 = coeff.intensity; - - matA(i, 0) = arx; - matA(i, 1) = ary; - matA(i, 2) = arz; - matA(i, 3) = atx; - matA(i, 4) = aty; - matA(i, 5) = atz; - matB(i, 0) = -0.05 * d2; - } - matAt = matA.transpose(); - matAtA = matAt * matA; - matAtB = matAt * matB; - - matX = matAtA.colPivHouseholderQr().solve(matAtB); - - if (iterCount == 0) { - Eigen::Matrix matE; - Eigen::Matrix matV; - Eigen::Matrix matV2; - - Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); - matE = esolver.eigenvalues().real(); - matV = esolver.eigenvectors().real(); - - matV2 = matV; - - isDegenerate = false; - float eignThre[6] = {10, 10, 10, 10, 10, 10}; - for (int i = 5; i >= 0; i--) { - if (matE(0, i) < eignThre[i]) { - for (int j = 0; j < 6; j++) { - matV2(i, j) = 0; - } - isDegenerate = true; - } else { - break; - } - } - matP = matV.inverse() * matV2; - } - - if (isDegenerate) { - Eigen::Matrix matX2; - matX2 = matX; - matX = matP * matX2; - } - - transform.rot_x = transform.rot_x.rad() + matX(0, 0); - transform.rot_y = transform.rot_y.rad() + matX(1, 0); - transform.rot_z = transform.rot_z.rad() + matX(2, 0); - transform.pos.x() += matX(3, 0); - transform.pos.y() += matX(4, 0); - transform.pos.z() += matX(5, 0); - - if( isnan(transform.rot_x.rad()) ) transform.rot_x = Angle(); - if( isnan(transform.rot_y.rad()) ) transform.rot_y = Angle(); - if( isnan(transform.rot_z.rad()) ) transform.rot_z = Angle(); - - if( isnan(transform.pos.x()) ) transform.pos.x() = 0.0; - if( isnan(transform.pos.y()) ) transform.pos.y() = 0.0; - if( isnan(transform.pos.z()) ) transform.pos.z() = 0.0; - - float deltaR = sqrt( - pow(rad2deg(matX(0, 0)), 2) + - pow(rad2deg(matX(1, 0)), 2) + - pow(rad2deg(matX(2, 0)), 2)); - float deltaT = sqrt( - pow(matX(3, 0) * 100, 2) + - pow(matX(4, 0) * 100, 2) + - pow(matX(5, 0) * 100, 2)); - - if (deltaR < 0.1 && deltaT < 0.1) { - break; - } - } - } - - Angle rx, ry, rz; - - AccumulateRotation(transformSum.rot_x, - transformSum.rot_y, - transformSum.rot_z, - -transform.rot_x, - -transform.rot_y.rad() * 1.05, - -transform.rot_z, - rx, ry, rz); - - Vector3 v( transform.pos.x() - imuShiftFromStart.x(), - transform.pos.y() - imuShiftFromStart.y(), - transform.pos.z()*1.05 - imuShiftFromStart.z() ); - rotateZXY(v, rz, rx, ry); - Vector3 trans = transformSum.pos - v; - - PluginIMURotation(rx, ry, rz, - imuPitchStart, imuYawStart, imuRollStart, - imuPitchLast, imuYawLast, imuRollLast, - rx, ry, rz); - - transformSum.rot_x = rx; - transformSum.rot_y = ry; - transformSum.rot_z = rz; - transformSum.pos = trans; - - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); - - laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserOdometry.pose.pose.orientation.x = -geoQuat.y; - laserOdometry.pose.pose.orientation.y = -geoQuat.z; - laserOdometry.pose.pose.orientation.z = geoQuat.x; - laserOdometry.pose.pose.orientation.w = geoQuat.w; - laserOdometry.pose.pose.position.x = trans.x(); - laserOdometry.pose.pose.position.y = trans.y(); - laserOdometry.pose.pose.position.z = trans.z(); - pubLaserOdometry.publish(laserOdometry); - - laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat); - laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - laserOdometryTrans.setOrigin(tf::Vector3( trans.x(), trans.y(), trans.z()) ); - tfBroadcaster.sendTransform(laserOdometryTrans); - - int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); - for (int i = 0; i < cornerPointsLessSharpNum; i++) { - TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); - } - - int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); - for (int i = 0; i < surfPointsLessFlatNum; i++) { - TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); - } - - frameCount++; - if (frameCount >= skipFrameNum + 1) { - int laserCloudFullResNum = laserCloudFullRes->points.size(); - for (int i = 0; i < laserCloudFullResNum; i++) { - TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); - } - } - - std::swap(cornerPointsLessSharp, laserCloudCornerLast); - std::swap(surfPointsLessFlat, laserCloudSurfLast); - - laserCloudCornerLastNum = laserCloudCornerLast->points.size(); - laserCloudSurfLastNum = laserCloudSurfLast->points.size(); - - if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) - { - kdtreeCornerLast.setInputCloud(laserCloudCornerLast); - kdtreeSurfLast.setInputCloud(laserCloudSurfLast); - } - - if (frameCount >= skipFrameNum + 1) { - frameCount = 0; - - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudCornerLast2.header.frame_id = "/camera"; - pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudSurfLast2.header.frame_id = "/camera"; - pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudFullRes3.header.frame_id = "/camera"; - pubLaserCloudFullRes.publish(laserCloudFullRes3); - } - } - - status = ros::ok(); - rate.sleep(); - } - - return 0; -} diff --git a/src/laser_odometry_node.cpp b/src/laser_odometry_node.cpp new file mode 100644 index 0000000..b1187bd --- /dev/null +++ b/src/laser_odometry_node.cpp @@ -0,0 +1,20 @@ +#include +#include "lib/LaserOdometry.h" + + +/** Main node entry point. */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "laserOdometry"); + ros::NodeHandle node; + ros::NodeHandle privateNode("~"); + + loam::LaserOdometry laserOdom(0.1); + + if (laserOdom.setup(node, privateNode)) { + // initialization successful + laserOdom.spin(); + } + + return 0; +} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 5350aa7..744b869 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -4,5 +4,7 @@ add_library(loam ScanRegistration.h ScanRegistration.cpp VelodyneScanRegistration.h - VelodyneScanRegistration.cpp) + VelodyneScanRegistration.cpp + LaserOdometry.h + LaserOdometry.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp new file mode 100644 index 0000000..246ed9d --- /dev/null +++ b/src/lib/LaserOdometry.cpp @@ -0,0 +1,884 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "LaserOdometry.h" + +#include +#include +#include +#include + + +namespace loam { + +using std::sin; +using std::cos; +using std::asin; +using std::atan2; +using std::sqrt; +using std::fabs; +using std::pow; + + +LaserOdometry::LaserOdometry(const float& scanPeriod, + const uint16_t& ioRatio) + : _scanPeriod(scanPeriod), + _ioRatio(ioRatio), + _systemInited(false), + _frameCount(0), + _cornerPointsSharp(new pcl::PointCloud()), + _cornerPointsLessSharp(new pcl::PointCloud()), + _surfPointsFlat(new pcl::PointCloud()), + _surfPointsLessFlat(new pcl::PointCloud()), + _laserCloud(new pcl::PointCloud()), + _imuTrans(new pcl::PointCloud()), + _lastCornerCloud(new pcl::PointCloud()), + _lastSurfaceCloud(new pcl::PointCloud()), + _laserCloudOri(new pcl::PointCloud()), + _coeffSel(new pcl::PointCloud()) +{ + // initialize odometry and odometry tf messages + _laserOdometry.header.frame_id = "/camera_init"; + _laserOdometry.child_frame_id = "/laser_odom"; + + _laserOdometryTrans.frame_id_ = "/camera_init"; + _laserOdometryTrans.child_frame_id_ = "/laser_odom"; +} + + + +bool LaserOdometry::setup(ros::NodeHandle &node, + ros::NodeHandle &privateNode) +{ + // advertise laser odometry topics + _pubLaserCloudCornerLast = node.advertise("/laser_cloud_corner_last", 2); + _pubLaserCloudSurfLast = node.advertise("/laser_cloud_surf_last", 2); + _pubLaserCloudFullRes = node.advertise("/velodyne_cloud_3", 2); + _pubLaserOdometry = node.advertise("/laser_odom_to_init", 5); + + // subscribe to scan registration topics + _subCornerPointsSharp = node.subscribe + ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this); + + _subCornerPointsLessSharp = node.subscribe + ("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this); + + _subSurfPointsFlat = node.subscribe + ("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this); + + _subSurfPointsLessFlat = node.subscribe + ("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this); + + _subLaserCloudFullRes = node.subscribe + ("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this); + + _subImuTrans = node.subscribe + ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this); + + // set active mode + _activeMode = true; + + return true; +} + + + +void LaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po) +{ + float s = 10 * (pi.intensity - int(pi.intensity)); + + po.x = pi.x - s * _transform.pos.x(); + po.y = pi.y - s * _transform.pos.y(); + po.z = pi.z - s * _transform.pos.z(); + po.intensity = pi.intensity; + + Angle rx = s * _transform.rot_x.rad(); + Angle ry = s * _transform.rot_y.rad(); + Angle rz = s * _transform.rot_z.rad(); + rotateZXY(po, -rz, -rx, -ry); +} + + + +size_t LaserOdometry::transformToEnd(pcl::PointCloud::Ptr& cloud) +{ + size_t cloudSize = cloud->points.size(); + + for (size_t i = 0; i < cloudSize; i++) { + pcl::PointXYZI& point = cloud->points[i]; + + float s = 10 * (point.intensity - int(point.intensity)); + + point.x -= s * _transform.pos.x(); + point.y -= s * _transform.pos.y(); + point.z -= s * _transform.pos.z(); + point.intensity = int(point.intensity); + + Angle rx = s * _transform.rot_x.rad(); + Angle ry = s * _transform.rot_y.rad(); + Angle rz = s * _transform.rot_z.rad(); + rotateZXY(point, -rz, -rx, -ry); + rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z); + + point.x += _transform.pos.x() - _imuShiftFromStart.x(); + point.y += _transform.pos.y() - _imuShiftFromStart.y(); + point.z += _transform.pos.z() - _imuShiftFromStart.z(); + + rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart); + rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd); + } + + return cloudSize; +} + + + +void LaserOdometry::pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, + const Angle& blx, const Angle& bly, const Angle& blz, + const Angle& alx, const Angle& aly, const Angle& alz, + Angle &acx, Angle &acy, Angle &acz) +{ + float sbcx = bcx.sin(); + float cbcx = bcx.cos(); + float sbcy = bcy.sin(); + float cbcy = bcy.cos(); + float sbcz = bcz.sin(); + float cbcz = bcz.cos(); + + float sblx = blx.sin(); + float cblx = blx.cos(); + float sbly = bly.sin(); + float cbly = bly.cos(); + float sblz = blz.sin(); + float cblz = blz.cos(); + + float salx = alx.sin(); + float calx = alx.cos(); + float saly = aly.sin(); + float caly = aly.cos(); + float salz = alz.sin(); + float calz = alz.cos(); + + float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) + - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); + acx = -asin(srx); + + float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + acy = atan2(srycrx / acx.cos(), crycrx / acx.cos()); + + float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) + - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) + - calx*cblx*cblz*salz) + + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + + calx*cblx*salz*sblz); + float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + + calx*calz*cblx*cblz) + - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) + - calx*calz*cblx*sblz); + acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos()); +} + + + +void LaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz, + Angle lx, Angle ly, Angle lz, + Angle &ox, Angle &oy, Angle &oz) +{ + float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin() + - cx.cos()*cz.cos()*lx.sin() + - lx.cos()*ly.cos()*cx.sin(); + ox = -asin(srx); + + float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin()) + + lx.cos()*ly.sin()*(cy.cos()*cz.cos() + cx.sin()*cy.sin()*cz.sin()) + + lx.cos()*ly.cos()*cx.cos()*cy.sin(); + float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos() + - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - cy.cos()*cx.sin()*cz.sin()) + - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin()); + oy = atan2(srycrx / ox.cos(), crycrx / ox.cos()); + + float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin()) + + cx.cos()*cz.sin()*(ly.cos()*lz.cos() + lx.sin()*ly.sin()*lz.sin()) + + lx.cos()*cx.cos()*cz.cos()*lz.sin(); + float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos() + - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - lz.cos()*lx.sin()*ly.sin()) + - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin()); + oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos()); +} + + + +void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg) +{ + _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp.toSec(); + + _cornerPointsSharp->clear(); + pcl::fromROSMsg(*cornerPointsSharpMsg, *_cornerPointsSharp); + std::vector indices; + pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices); + _newCornerPointsSharp = true; +} + + + +void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg) +{ + _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp.toSec(); + + _cornerPointsLessSharp->clear(); + pcl::fromROSMsg(*cornerPointsLessSharpMsg, *_cornerPointsLessSharp); + std::vector indices; + pcl::removeNaNFromPointCloud(*_cornerPointsLessSharp, *_cornerPointsLessSharp, indices); + _newCornerPointsLessSharp = true; +} + + + +void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg) +{ + _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp.toSec(); + + _surfPointsFlat->clear(); + pcl::fromROSMsg(*surfPointsFlatMsg, *_surfPointsFlat); + std::vector indices; + pcl::removeNaNFromPointCloud(*_surfPointsFlat, *_surfPointsFlat, indices); + _newSurfPointsFlat = true; +} + + + +void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg) +{ + _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp.toSec(); + + _surfPointsLessFlat->clear(); + pcl::fromROSMsg(*surfPointsLessFlatMsg, *_surfPointsLessFlat); + std::vector indices; + pcl::removeNaNFromPointCloud(*_surfPointsLessFlat, *_surfPointsLessFlat, indices); + _newSurfPointsLessFlat = true; +} + + + +void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) +{ + _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec(); + + _laserCloud->clear(); + pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloud); + std::vector indices; + pcl::removeNaNFromPointCloud(*_laserCloud, *_laserCloud, indices); + _newLaserCloudFullRes = true; +} + + + +void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg) +{ + _timeImuTrans = imuTransMsg->header.stamp.toSec(); + + _imuTrans->clear(); + pcl::fromROSMsg(*imuTransMsg, *_imuTrans); + + _imuPitchStart = _imuTrans->points[0].x; + _imuYawStart = _imuTrans->points[0].y; + _imuRollStart = _imuTrans->points[0].z; + + _imuPitchEnd = _imuTrans->points[1].x; + _imuYawEnd = _imuTrans->points[1].y; + _imuRollEnd = _imuTrans->points[1].z; + + _imuShiftFromStart = _imuTrans->points[2]; + _imuVeloFromStart = _imuTrans->points[3]; + + _newImuTrans = true; +} + + + +void LaserOdometry::spin() +{ + if (!_activeMode) { + return; + } + + ros::Rate rate(100); + bool status = ros::ok(); + + // loop until shutdown + while (status) { + ros::spinOnce(); + + // try processing new data + process(); + + status = ros::ok(); + rate.sleep(); + } +} + + + +void LaserOdometry::reset() +{ + _newCornerPointsSharp = false; + _newCornerPointsLessSharp = false; + _newSurfPointsFlat = false; + _newSurfPointsLessFlat = false; + _newLaserCloudFullRes = false; + _newImuTrans = false; +} + + + +bool LaserOdometry::hasNewData() +{ + return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat && + _newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans && + fabs(_timeCornerPointsSharp - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeCornerPointsLessSharp - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeSurfPointsFlat - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeLaserCloudFullRes - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeImuTrans - _timeSurfPointsLessFlat) < 0.005; +} + + + +void LaserOdometry::process() +{ + if (!hasNewData()) { + // waiting for new data to arrive... + return; + } + + // reset flags, etc. + reset(); + + if (!_systemInited) { + _cornerPointsLessSharp.swap(_lastCornerCloud); + _surfPointsLessFlat.swap(_lastSurfaceCloud); + + _lastCornerCloudSize = _lastCornerCloud->points.size(); + _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + + _lastCornerKDTree.setInputCloud(_lastCornerCloud); + _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); + + _transformSum.rot_x += _imuPitchStart; + _transformSum.rot_z += _imuRollStart; + + _systemInited = true; + return; + } + + pcl::PointXYZI coeff; + bool isDegenerate = false; + Eigen::Matrix matP; + + _frameCount++; + _transform.pos -= _imuVeloFromStart * _scanPeriod; + + if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + std::vector pointSearchInd(1); + std::vector pointSearchSqDis(1); + std::vector indices; + + pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices); + size_t cornerPointsSharpNum = _cornerPointsSharp->points.size(); + size_t surfPointsFlatNum = _surfPointsFlat->points.size(); + + _pointSearchCornerInd1.resize(cornerPointsSharpNum); + _pointSearchCornerInd2.resize(cornerPointsSharpNum); + _pointSearchSurfInd1.resize(surfPointsFlatNum); + _pointSearchSurfInd2.resize(surfPointsFlatNum); + _pointSearchSurfInd3.resize(surfPointsFlatNum); + + for (int iterCount = 0; iterCount < 25; iterCount++) { + pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3; + _laserCloudOri->clear(); + _coeffSel->clear(); + + for (int i = 0; i < cornerPointsSharpNum; i++) { + transformToStart(_cornerPointsSharp->points[i], pointSel); + + if (iterCount % 5 == 0) { + pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices); + _lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); + + int closestPointInd = -1, minPointInd2 = -1; + if (pointSearchSqDis[0] < 25) { + closestPointInd = pointSearchInd[0]; + int closestPointScan = int(_lastCornerCloud->points[closestPointInd].intensity); + + float pointSqDis, minPointSqDis2 = 25; + for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { + if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); + + if (int(_lastCornerCloud->points[j].intensity) > closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } + } + for (int j = closestPointInd - 1; j >= 0; j--) { + if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); + + if (int(_lastCornerCloud->points[j].intensity) < closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } + } + } + + _pointSearchCornerInd1[i] = closestPointInd; + _pointSearchCornerInd2[i] = minPointInd2; + } + + if (_pointSearchCornerInd2[i] >= 0) { + tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]]; + tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]]; + + float x0 = pointSel.x; + float y0 = pointSel.y; + float z0 = pointSel.z; + float x1 = tripod1.x; + float y1 = tripod1.y; + float z1 = tripod1.z; + float x2 = tripod2.x; + float y2 = tripod2.y; + float z2 = tripod2.z; + + float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) + * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); + + float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); + + float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; + + float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float ld2 = a012 / l12; + + // TODO: Why writing to a variable that's never read? + pointProj = pointSel; + pointProj.x -= la * ld2; + pointProj.y -= lb * ld2; + pointProj.z -= lc * ld2; + + float s = 1; + if (iterCount >= 5) { + s = 1 - 1.8f * fabs(ld2); + } + + coeff.x = s * la; + coeff.y = s * lb; + coeff.z = s * lc; + coeff.intensity = s * ld2; + + if (s > 0.1 && ld2 != 0) { + _laserCloudOri->push_back(_cornerPointsSharp->points[i]); + _coeffSel->push_back(coeff); + } + } + } + + for (int i = 0; i < surfPointsFlatNum; i++) { + transformToStart(_surfPointsFlat->points[i], pointSel); + + if (iterCount % 5 == 0) { + _lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); + int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; + if (pointSearchSqDis[0] < 25) { + closestPointInd = pointSearchInd[0]; + int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity); + + float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; + for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { + if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); + + if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } else { + if (pointSqDis < minPointSqDis3) { + minPointSqDis3 = pointSqDis; + minPointInd3 = j; + } + } + } + for (int j = closestPointInd - 1; j >= 0; j--) { + if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); + + if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } else { + if (pointSqDis < minPointSqDis3) { + minPointSqDis3 = pointSqDis; + minPointInd3 = j; + } + } + } + } + + _pointSearchSurfInd1[i] = closestPointInd; + _pointSearchSurfInd2[i] = minPointInd2; + _pointSearchSurfInd3[i] = minPointInd3; + } + + if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0) { + tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]]; + tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]]; + tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]]; + + float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) + - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); + float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) + - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); + float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) + - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); + float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); + + float ps = sqrt(pa * pa + pb * pb + pc * pc); + pa /= ps; + pb /= ps; + pc /= ps; + pd /= ps; + + float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; + + // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards? + pointProj = pointSel; + pointProj.x -= pa * pd2; + pointProj.y -= pb * pd2; + pointProj.z -= pc * pd2; + + float s = 1; + if (iterCount >= 5) { + s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel)); + } + + coeff.x = s * pa; + coeff.y = s * pb; + coeff.z = s * pc; + coeff.intensity = s * pd2; + + if (s > 0.1 && pd2 != 0) { + _laserCloudOri->push_back(_surfPointsFlat->points[i]); + _coeffSel->push_back(coeff); + } + } + } + + int pointSelNum = _laserCloudOri->points.size(); + if (pointSelNum < 10) { + continue; + } + + Eigen::Matrix matA(pointSelNum, 6); + Eigen::Matrix matAt(6,pointSelNum); + Eigen::Matrix matAtA; + Eigen::VectorXf matB(pointSelNum); + Eigen::Matrix matAtB; + Eigen::Matrix matX; + + for (int i = 0; i < pointSelNum; i++) { + const pcl::PointXYZI& pointOri = _laserCloudOri->points[i]; + coeff = _coeffSel->points[i]; + + float s = 1; + + float srx = sin(s * _transform.rot_x.rad()); + float crx = cos(s * _transform.rot_x.rad()); + float sry = sin(s * _transform.rot_y.rad()); + float cry = cos(s * _transform.rot_y.rad()); + float srz = sin(s * _transform.rot_z.rad()); + float crz = cos(s * _transform.rot_z.rad()); + float tx = s * _transform.pos.x(); + float ty = s * _transform.pos.y(); + float tz = s * _transform.pos.z(); + + float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z + + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x + + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z + + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y + + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z + + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z; + + float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x + + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z + + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) + + s*tz*crx*cry) * coeff.x + + ((s*cry*crz - s*srx*sry*srz)*pointOri.x + + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z + + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) + - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z; + + float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y + + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x + + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y + + s*ty*crx*srz + s*tx*crx*crz) * coeff.y + + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y + + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z; + + float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y + - s*(crz*sry + cry*srx*srz) * coeff.z; + + float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y + - s*(sry*srz - cry*crz*srx) * coeff.z; + + float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z; + + float d2 = coeff.intensity; + + matA(i, 0) = arx; + matA(i, 1) = ary; + matA(i, 2) = arz; + matA(i, 3) = atx; + matA(i, 4) = aty; + matA(i, 5) = atz; + matB(i, 0) = -0.05 * d2; + } + matAt = matA.transpose(); + matAtA = matAt * matA; + matAtB = matAt * matB; + + matX = matAtA.colPivHouseholderQr().solve(matAtB); + + if (iterCount == 0) { + Eigen::Matrix matE; + Eigen::Matrix matV; + Eigen::Matrix matV2; + + Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); + matE = esolver.eigenvalues().real(); + matV = esolver.eigenvectors().real(); + + matV2 = matV; + + isDegenerate = false; + float eignThre[6] = {10, 10, 10, 10, 10, 10}; + for (int i = 5; i >= 0; i--) { + if (matE(0, i) < eignThre[i]) { + for (int j = 0; j < 6; j++) { + matV2(i, j) = 0; + } + isDegenerate = true; + } else { + break; + } + } + matP = matV.inverse() * matV2; + } + + if (isDegenerate) { + Eigen::Matrix matX2; + matX2 = matX; + matX = matP * matX2; + } + + _transform.rot_x = _transform.rot_x.rad() + matX(0, 0); + _transform.rot_y = _transform.rot_y.rad() + matX(1, 0); + _transform.rot_z = _transform.rot_z.rad() + matX(2, 0); + _transform.pos.x() += matX(3, 0); + _transform.pos.y() += matX(4, 0); + _transform.pos.z() += matX(5, 0); + + if( isnan(_transform.rot_x.rad()) ) _transform.rot_x = Angle(); + if( isnan(_transform.rot_y.rad()) ) _transform.rot_y = Angle(); + if( isnan(_transform.rot_z.rad()) ) _transform.rot_z = Angle(); + + if( isnan(_transform.pos.x()) ) _transform.pos.x() = 0.0; + if( isnan(_transform.pos.y()) ) _transform.pos.y() = 0.0; + if( isnan(_transform.pos.z()) ) _transform.pos.z() = 0.0; + + float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) + + pow(rad2deg(matX(1, 0)), 2) + + pow(rad2deg(matX(2, 0)), 2)); + float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + + pow(matX(4, 0) * 100, 2) + + pow(matX(5, 0) * 100, 2)); + + if (deltaR < 0.1 && deltaT < 0.1) { + break; + } + } + } + + Angle rx, ry, rz; + + accumulateRotation(_transformSum.rot_x, + _transformSum.rot_y, + _transformSum.rot_z, + -_transform.rot_x, + -_transform.rot_y.rad() * 1.05, + -_transform.rot_z, + rx, ry, rz); + + Vector3 v( _transform.pos.x() - _imuShiftFromStart.x(), + _transform.pos.y() - _imuShiftFromStart.y(), + _transform.pos.z() * 1.05 - _imuShiftFromStart.z() ); + rotateZXY(v, rz, rx, ry); + Vector3 trans = _transformSum.pos - v; + + pluginIMURotation(rx, ry, rz, + _imuPitchStart, _imuYawStart, _imuRollStart, + _imuPitchEnd, _imuYawEnd, _imuRollEnd, + rx, ry, rz); + + _transformSum.rot_x = rx; + _transformSum.rot_y = ry; + _transformSum.rot_z = rz; + _transformSum.pos = trans; + + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); + + _laserOdometry.header.stamp = ros::Time().fromSec(_timeSurfPointsLessFlat); + _laserOdometry.pose.pose.orientation.x = -geoQuat.y; + _laserOdometry.pose.pose.orientation.y = -geoQuat.z; + _laserOdometry.pose.pose.orientation.z = geoQuat.x; + _laserOdometry.pose.pose.orientation.w = geoQuat.w; + _laserOdometry.pose.pose.position.x = trans.x(); + _laserOdometry.pose.pose.position.y = trans.y(); + _laserOdometry.pose.pose.position.z = trans.z(); + if (_activeMode) { + _pubLaserOdometry.publish(_laserOdometry); + } + + _laserOdometryTrans.stamp_ = ros::Time().fromSec(_timeSurfPointsLessFlat); + _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + _laserOdometryTrans.setOrigin(tf::Vector3( trans.x(), trans.y(), trans.z()) ); + _tfBroadcaster.sendTransform(_laserOdometryTrans); + + transformToEnd(_cornerPointsLessSharp); + transformToEnd(_surfPointsLessFlat); + + _cornerPointsLessSharp.swap(_lastCornerCloud); + _surfPointsLessFlat.swap(_lastSurfaceCloud); + + _lastCornerCloudSize = _lastCornerCloud->points.size(); + _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + + if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + _lastCornerKDTree.setInputCloud(_lastCornerCloud); + _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); + } + + publishResult(); +} + + + +void LaserOdometry::publishResult() +{ + // only publish messages in active mode + if (!_activeMode) { + // transform each full resolution cloud in passive mode as we don't know who will be using it + transformToEnd(_laserCloud); + return; + } + + // publish results according to the input output ratio + if (_ioRatio < 2 || _frameCount % _ioRatio == 1) { + ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); + + sensor_msgs::PointCloud2 laserCloudCornerLast2; + pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); + laserCloudCornerLast2.header.stamp = sweepTime; + laserCloudCornerLast2.header.frame_id = "/camera"; + _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); + + sensor_msgs::PointCloud2 laserCloudSurfLast2; + pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); + laserCloudSurfLast2.header.stamp = sweepTime; + laserCloudSurfLast2.header.frame_id = "/camera"; + _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); + + sensor_msgs::PointCloud2 laserCloudFullRes3; + transformToEnd(_laserCloud); // transform full resolution cloud before sending it + pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = sweepTime; + laserCloudFullRes3.header.frame_id = "/camera"; + _pubLaserCloudFullRes.publish(laserCloudFullRes3); + } +} + +} // end namespace loam diff --git a/src/lib/LaserOdometry.h b/src/lib/LaserOdometry.h new file mode 100644 index 0000000..8bca58f --- /dev/null +++ b/src/lib/LaserOdometry.h @@ -0,0 +1,217 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_LASERODOMETRY_H +#define LOAM_LASERODOMETRY_H + +#include "math_utils.h" +#include "loam_velodyne/nanoflann_pcl.h" + +#include +#include +#include +#include +#include +#include +#include + + +namespace loam { + +/** \brief Implementation of the LOAM laser odometry component. + * + */ +class LaserOdometry { +public: + LaserOdometry(const float& scanPeriod, + const uint16_t& ioRatio = 2); + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + virtual bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Handler method for a new sharp corner cloud. + * + * @param cornerPointsSharpMsg the new sharp corner cloud message + */ + void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg); + + /** \brief Handler method for a new less sharp corner cloud. + * + * @param cornerPointsLessSharpMsg the new less sharp corner cloud message + */ + void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg); + + /** \brief Handler method for a new flat surface cloud. + * + * @param surfPointsFlatMsg the new flat surface cloud message + */ + void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg); + + /** \brief Handler method for a new less flat surface cloud. + * + * @param surfPointsLessFlatMsg the new less flat surface cloud message + */ + void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg); + + /** \brief Handler method for a new full resolution cloud. + * + * @param laserCloudFullResMsg the new full resolution cloud message + */ + void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg); + + /** \brief Handler method for a new IMU transformation information. + * + * @param laserCloudFullResMsg the new IMU transformation information message + */ + void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg); + + /** \brief Process incoming messages in a loop until shutdown (used in active mode). */ + void spin(); + + /** \brief Try to process buffered data. */ + void process(); + + +protected: + /** \brief Reset flags, etc. */ + void reset(); + + /** \brief Check if all required information for a new processing step is available. */ + bool hasNewData(); + + /** \brief Transform the given point to the start of the sweep. + * + * @param pi the point to transform + * @param po the point instance for storing the result + */ + void transformToStart(const pcl::PointXYZI& pi, + pcl::PointXYZI& po); + + /** \brief Transform the given point cloud to the end of the sweep. + * + * @param cloud the point cloud to transform + */ + size_t transformToEnd(pcl::PointCloud::Ptr& cloud); + + void pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, + const Angle& blx, const Angle& bly, const Angle& blz, + const Angle& alx, const Angle& aly, const Angle& alz, + Angle &acx, Angle &acy, Angle &acz); + + void accumulateRotation(Angle cx, Angle cy, Angle cz, + Angle lx, Angle ly, Angle lz, + Angle &ox, Angle &oy, Angle &oz); + + /** \brief Publish the current result via the respective topics. */ + void publishResult(); + +private: + const float _scanPeriod; ///< time per scan + const uint16_t _ioRatio; ///< ratio of input to output frames + bool _systemInited; ///< initialization flag + long _frameCount; ///< number of processed frames + + pcl::PointCloud::Ptr _cornerPointsSharp; ///< sharp corner points cloud + pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud + pcl::PointCloud::Ptr _surfPointsFlat; ///< flat surface points cloud + pcl::PointCloud::Ptr _surfPointsLessFlat; ///< less flat surface points cloud + pcl::PointCloud::Ptr _laserCloud; ///< full resolution cloud + pcl::PointCloud::Ptr _imuTrans; ///< IMU transformation information + + pcl::PointCloud::Ptr _lastCornerCloud; ///< last corner points cloud + pcl::PointCloud::Ptr _lastSurfaceCloud; ///< last surface points cloud + pcl::PointCloud::Ptr _laserCloudOri; ///< point selection + pcl::PointCloud::Ptr _coeffSel; ///< point selection coefficients + + nanoflann::KdTreeFLANN _lastCornerKDTree; ///< last corner cloud KD-tree + nanoflann::KdTreeFLANN _lastSurfaceKDTree; ///< last surface cloud KD-tree + + double _timeCornerPointsSharp; ///< time of current sharp corner cloud + double _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud + double _timeSurfPointsFlat; ///< time of current flat surface cloud + double _timeSurfPointsLessFlat; ///< time of current less flat surface cloud + double _timeLaserCloudFullRes; ///< time of current full resolution cloud + double _timeImuTrans; ///< time of current IMU transformation information + + bool _newCornerPointsSharp; ///< flag if a new sharp corner cloud has been received + bool _newCornerPointsLessSharp; ///< flag if a new less sharp corner cloud has been received + bool _newSurfPointsFlat; ///< flag if a new flat surface cloud has been received + bool _newSurfPointsLessFlat; ///< flag if a new less flat surface cloud has been received + bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received + bool _newImuTrans; ///< flag if a new IMU transformation information cloud has been received + + nav_msgs::Odometry _laserOdometry; ///< laser odometry message + tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation + + size_t _lastCornerCloudSize; ///< size of the last corner cloud + size_t _lastSurfaceCloudSize; ///< size of the last surface cloud + + std::vector _pointSearchCornerInd1; ///< first corner point search index buffer + std::vector _pointSearchCornerInd2; ///< second corner point search index buffer + + std::vector _pointSearchSurfInd1; ///< first surface point search index buffer + std::vector _pointSearchSurfInd2; ///< second surface point search index buffer + std::vector _pointSearchSurfInd3; ///< third surface point search index buffer + + Twist _transform; ///< optimized pose transformation + Twist _transformSum; ///< accumulated optimized pose transformation + + Angle _imuRollStart, _imuPitchStart, _imuYawStart; + Angle _imuRollEnd, _imuPitchEnd, _imuYawEnd; + + Vector3 _imuShiftFromStart; + Vector3 _imuVeloFromStart; + + bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) + + ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher + ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher + ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher + ros::Publisher _pubLaserOdometry; ///< laser odometry publisher + tf::TransformBroadcaster _tfBroadcaster; ///< laser odometry transform broadcaster + + ros::Subscriber _subCornerPointsSharp; ///< sharp corner cloud message subscriber + ros::Subscriber _subCornerPointsLessSharp; ///< less sharp corner cloud message subscriber + ros::Subscriber _subSurfPointsFlat; ///< flat surface cloud message subscriber + ros::Subscriber _subSurfPointsLessFlat; ///< less flat surface cloud message subscriber + ros::Subscriber _subLaserCloudFullRes; ///< full resolution cloud message subscriber + ros::Subscriber _subImuTrans; ///< IMU transformation information message subscriber +}; + +} // end namespace loam + +#endif //LOAM_LASERODOMETRY_H diff --git a/src/lib/ScanRegistration.h b/src/lib/ScanRegistration.h index db6f677..008d418 100644 --- a/src/lib/ScanRegistration.h +++ b/src/lib/ScanRegistration.h @@ -246,7 +246,7 @@ typedef struct IMUState { -/** \brief Base class for scan registration implementations. +/** \brief Base class for LOAM scan registration implementations. * * As there exist various sensor devices, producing differently formatted point clouds, * specific implementations are needed for each group of sensor devices to achieve an accurate registration. diff --git a/src/lib/VelodyneScanRegistration.h b/src/lib/VelodyneScanRegistration.h index 4e4434e..f0852e2 100644 --- a/src/lib/VelodyneScanRegistration.h +++ b/src/lib/VelodyneScanRegistration.h @@ -39,6 +39,9 @@ namespace loam { +/** \brief Class for registering Velodyne VLP-16 scans. + * + */ class VelodyneScanRegistration : virtual public ScanRegistration { public: VelodyneScanRegistration(const float& scanPeriod, From d4c07826347cf1fe6ec82b14fc32a85da8d999b4 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Fri, 1 Dec 2017 18:59:38 +0100 Subject: [PATCH 3/9] Extracted transform maintenance component into own class. No changes to the logic. Active / passive modi added, but passive mode needs further work to be usable. --- CMakeLists.txt | 4 +- src/lib/CMakeLists.txt | 4 +- src/lib/TransformMaintenance.cpp | 241 +++++++++++++++++++++++++++++ src/lib/TransformMaintenance.h | 97 ++++++++++++ src/transformMaintenance.cpp | 228 --------------------------- src/transform_maintenance_node.cpp | 20 +++ 6 files changed, 363 insertions(+), 231 deletions(-) create mode 100644 src/lib/TransformMaintenance.cpp create mode 100644 src/lib/TransformMaintenance.h delete mode 100755 src/transformMaintenance.cpp create mode 100644 src/transform_maintenance_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c65a51b..a1298ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,8 +40,8 @@ target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) add_executable(laserMapping src/laserMapping.cpp) target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) -add_executable(transformMaintenance src/transformMaintenance.cpp) -target_link_libraries(transformMaintenance ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) +add_executable(transformMaintenance src/transform_maintenance_node.cpp) +target_link_libraries(transformMaintenance ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 744b869..70a256c 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -6,5 +6,7 @@ add_library(loam VelodyneScanRegistration.h VelodyneScanRegistration.cpp LaserOdometry.h - LaserOdometry.cpp) + LaserOdometry.cpp + TransformMaintenance.h + TransformMaintenance.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/TransformMaintenance.cpp b/src/lib/TransformMaintenance.cpp new file mode 100644 index 0000000..0eca7bb --- /dev/null +++ b/src/lib/TransformMaintenance.cpp @@ -0,0 +1,241 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "TransformMaintenance.h" + + +namespace loam { + +TransformMaintenance::TransformMaintenance() +{ + // initialize odometry and odometry tf messages + _laserOdometry2.header.frame_id = "/camera_init"; + _laserOdometry2.child_frame_id = "/camera"; + + _laserOdometryTrans2.frame_id_ = "/camera_init"; + _laserOdometryTrans2.child_frame_id_ = "/camera"; + + + for (int i = 0; i < 6; i++) { + _transformSum[i] = 0; + _transformIncre[i] = 0; + _transformMapped[i] = 0; + _transformBefMapped[i] = 0; + _transformAftMapped[i] = 0; + } +} + + +bool TransformMaintenance::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode) +{ + // advertise integrated laser odometry topic + _pubLaserOdometry2 = node.advertise ("/integrated_to_init", 5); + + // subscribe to laser odometry and mapping odometry topics + _subLaserOdometry = node.subscribe + ("/laser_odom_to_init", 5, &TransformMaintenance::laserOdometryHandler, this); + + _subOdomAftMapped = node.subscribe + ("/aft_mapped_to_init", 5, &TransformMaintenance::odomAftMappedHandler, this); + + // set active mode + _activeMode = true; + + return true; +} + + + +void TransformMaintenance::transformAssociateToMap() +{ + float x1 = cos(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3]) + - sin(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]); + float y1 = _transformBefMapped[4] - _transformSum[4]; + float z1 = sin(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3]) + + cos(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]); + + float x2 = x1; + float y2 = cos(_transformSum[0]) * y1 + sin(_transformSum[0]) * z1; + float z2 = -sin(_transformSum[0]) * y1 + cos(_transformSum[0]) * z1; + + _transformIncre[3] = cos(_transformSum[2]) * x2 + sin(_transformSum[2]) * y2; + _transformIncre[4] = -sin(_transformSum[2]) * x2 + cos(_transformSum[2]) * y2; + _transformIncre[5] = z2; + + float sbcx = sin(_transformSum[0]); + float cbcx = cos(_transformSum[0]); + float sbcy = sin(_transformSum[1]); + float cbcy = cos(_transformSum[1]); + float sbcz = sin(_transformSum[2]); + float cbcz = cos(_transformSum[2]); + + float sblx = sin(_transformBefMapped[0]); + float cblx = cos(_transformBefMapped[0]); + float sbly = sin(_transformBefMapped[1]); + float cbly = cos(_transformBefMapped[1]); + float sblz = sin(_transformBefMapped[2]); + float cblz = cos(_transformBefMapped[2]); + + float salx = sin(_transformAftMapped[0]); + float calx = cos(_transformAftMapped[0]); + float saly = sin(_transformAftMapped[1]); + float caly = cos(_transformAftMapped[1]); + float salz = sin(_transformAftMapped[2]); + float calz = cos(_transformAftMapped[2]); + + float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) + - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + + cblx*salx*sbly) + - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + + cblx*cbly*salx); + _transformMapped[0] = -asin(srx); + + float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) + - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) + - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) + - calx*cblx*cbly*saly) + + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + + calx*cblx*saly*sbly); + float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) + - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) + + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + + calx*caly*cblx*cbly) + - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) + - calx*caly*cblx*sbly); + _transformMapped[1] = atan2(srycrx / cos(_transformMapped[0]), + crycrx / cos(_transformMapped[0])); + + float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + + cblx*cbly*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + + cblx*salx*sbly) + + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + + cblx*salx*sbly) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + + cblx*cbly*salx) + + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + _transformMapped[2] = atan2(srzcrx / cos(_transformMapped[0]), + crzcrx / cos(_transformMapped[0])); + + x1 = cos(_transformMapped[2]) * _transformIncre[3] - sin(_transformMapped[2]) * _transformIncre[4]; + y1 = sin(_transformMapped[2]) * _transformIncre[3] + cos(_transformMapped[2]) * _transformIncre[4]; + z1 = _transformIncre[5]; + + x2 = x1; + y2 = cos(_transformMapped[0]) * y1 - sin(_transformMapped[0]) * z1; + z2 = sin(_transformMapped[0]) * y1 + cos(_transformMapped[0]) * z1; + + _transformMapped[3] = _transformAftMapped[3] + - (cos(_transformMapped[1]) * x2 + sin(_transformMapped[1]) * z2); + _transformMapped[4] = _transformAftMapped[4] - y2; + _transformMapped[5] = _transformAftMapped[5] + - (-sin(_transformMapped[1]) * x2 + cos(_transformMapped[1]) * z2); +} + + + +void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) +{ + double roll, pitch, yaw; + geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); + + _transformSum[0] = -pitch; + _transformSum[1] = -yaw; + _transformSum[2] = roll; + + _transformSum[3] = laserOdometry->pose.pose.position.x; + _transformSum[4] = laserOdometry->pose.pose.position.y; + _transformSum[5] = laserOdometry->pose.pose.position.z; + + transformAssociateToMap(); + + geoQuat = tf::createQuaternionMsgFromRollPitchYaw + (_transformMapped[2], -_transformMapped[0], -_transformMapped[1]); + + _laserOdometry2.header.stamp = laserOdometry->header.stamp; + _laserOdometry2.pose.pose.orientation.x = -geoQuat.y; + _laserOdometry2.pose.pose.orientation.y = -geoQuat.z; + _laserOdometry2.pose.pose.orientation.z = geoQuat.x; + _laserOdometry2.pose.pose.orientation.w = geoQuat.w; + _laserOdometry2.pose.pose.position.x = _transformMapped[3]; + _laserOdometry2.pose.pose.position.y = _transformMapped[4]; + _laserOdometry2.pose.pose.position.z = _transformMapped[5]; + + if (_activeMode) { + // only publish messages in active mode + _pubLaserOdometry2.publish(_laserOdometry2); + } + + _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; + _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + _laserOdometryTrans2.setOrigin(tf::Vector3(_transformMapped[3], _transformMapped[4], _transformMapped[5])); + _tfBroadcaster2.sendTransform(_laserOdometryTrans2); +} + + + +void TransformMaintenance::odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) +{ + double roll, pitch, yaw; + geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); + + _transformAftMapped[0] = -pitch; + _transformAftMapped[1] = -yaw; + _transformAftMapped[2] = roll; + + _transformAftMapped[3] = odomAftMapped->pose.pose.position.x; + _transformAftMapped[4] = odomAftMapped->pose.pose.position.y; + _transformAftMapped[5] = odomAftMapped->pose.pose.position.z; + + _transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; + _transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; + _transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; + + _transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; + _transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; + _transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; +} + +} // end namespace loam diff --git a/src/lib/TransformMaintenance.h b/src/lib/TransformMaintenance.h new file mode 100644 index 0000000..7ffa85a --- /dev/null +++ b/src/lib/TransformMaintenance.h @@ -0,0 +1,97 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_TRANSFORMMAINTENANCE_H +#define LOAM_TRANSFORMMAINTENANCE_H + + +#include +#include +#include + +namespace loam { + +/** \brief Implementation of the LOAM transformation maintenance component. + * + */ +class TransformMaintenance { +public: + TransformMaintenance(); + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + virtual bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Handler method for laser odometry messages. + * + * @param laserOdometry the new laser odometry + */ + void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); + + /** \brief Handler method for mapping odometry messages. + * + * @param odomAftMapped the new mapping odometry + */ + void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped); + + +protected: + void transformAssociateToMap(); + + +private: + float _transformSum[6]; + float _transformIncre[6]; + float _transformMapped[6]; + float _transformBefMapped[6]; + float _transformAftMapped[6]; + + nav_msgs::Odometry _laserOdometry2; ///< latest integrated laser odometry message + tf::StampedTransform _laserOdometryTrans2; ///< latest integrated laser odometry transformation + + ros::Publisher _pubLaserOdometry2; ///< integrated laser odometry publisher + tf::TransformBroadcaster _tfBroadcaster2; ///< integrated laser odometry transformation broadcaster + + ros::Subscriber _subLaserOdometry; ///< (high frequency) laser odometry subscriber + ros::Subscriber _subOdomAftMapped; ///< (low frequency) mapping odometry subscriber + + bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) +}; + +} // end namespace loam + + +#endif //LOAM_TRANSFORMMAINTENANCE_H diff --git a/src/transformMaintenance.cpp b/src/transformMaintenance.cpp deleted file mode 100755 index e2fa8a3..0000000 --- a/src/transformMaintenance.cpp +++ /dev/null @@ -1,228 +0,0 @@ -// Copyright 2013, Ji Zhang, Carnegie Mellon University -// Further contributions copyright (c) 2016, Southwest Research Institute -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This is an implementation of the algorithm described in the following paper: -// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. -// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -float transformSum[6] = {0}; -float transformIncre[6] = {0}; -float transformMapped[6] = {0}; -float transformBefMapped[6] = {0}; -float transformAftMapped[6] = {0}; - -ros::Publisher *pubLaserOdometry2Pointer = NULL; -tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; -nav_msgs::Odometry laserOdometry2; -tf::StampedTransform laserOdometryTrans2; - -void transformAssociateToMap() -{ - float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); - float y1 = transformBefMapped[4] - transformSum[4]; - float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); - - float x2 = x1; - float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; - float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; - - transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; - transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; - transformIncre[5] = z2; - - float sbcx = sin(transformSum[0]); - float cbcx = cos(transformSum[0]); - float sbcy = sin(transformSum[1]); - float cbcy = cos(transformSum[1]); - float sbcz = sin(transformSum[2]); - float cbcz = cos(transformSum[2]); - - float sblx = sin(transformBefMapped[0]); - float cblx = cos(transformBefMapped[0]); - float sbly = sin(transformBefMapped[1]); - float cbly = cos(transformBefMapped[1]); - float sblz = sin(transformBefMapped[2]); - float cblz = cos(transformBefMapped[2]); - - float salx = sin(transformAftMapped[0]); - float calx = cos(transformAftMapped[0]); - float saly = sin(transformAftMapped[1]); - float caly = cos(transformAftMapped[1]); - float salz = sin(transformAftMapped[2]); - float calz = cos(transformAftMapped[2]); - - float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) - - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); - transformMapped[0] = -asin(srx); - - float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) - - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) - - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) - + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) - + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) - + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); - float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) - - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) - + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) - + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) - - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) - + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); - transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), - crycrx / cos(transformMapped[0])); - - float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) - - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); - float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) - + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); - transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), - crzcrx / cos(transformMapped[0])); - - x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; - y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; - z1 = transformIncre[5]; - - x2 = x1; - y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; - z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; - - transformMapped[3] = transformAftMapped[3] - - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); - transformMapped[4] = transformAftMapped[4] - y2; - transformMapped[5] = transformAftMapped[5] - - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); -} - -void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) -{ - double roll, pitch, yaw; - geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; - tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); - - transformSum[0] = -pitch; - transformSum[1] = -yaw; - transformSum[2] = roll; - - transformSum[3] = laserOdometry->pose.pose.position.x; - transformSum[4] = laserOdometry->pose.pose.position.y; - transformSum[5] = laserOdometry->pose.pose.position.z; - - transformAssociateToMap(); - - geoQuat = tf::createQuaternionMsgFromRollPitchYaw - (transformMapped[2], -transformMapped[0], -transformMapped[1]); - - laserOdometry2.header.stamp = laserOdometry->header.stamp; - laserOdometry2.pose.pose.orientation.x = -geoQuat.y; - laserOdometry2.pose.pose.orientation.y = -geoQuat.z; - laserOdometry2.pose.pose.orientation.z = geoQuat.x; - laserOdometry2.pose.pose.orientation.w = geoQuat.w; - laserOdometry2.pose.pose.position.x = transformMapped[3]; - laserOdometry2.pose.pose.position.y = transformMapped[4]; - laserOdometry2.pose.pose.position.z = transformMapped[5]; - pubLaserOdometry2Pointer->publish(laserOdometry2); - - laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; - laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); - tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); -} - -void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) -{ - double roll, pitch, yaw; - geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; - tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); - - transformAftMapped[0] = -pitch; - transformAftMapped[1] = -yaw; - transformAftMapped[2] = roll; - - transformAftMapped[3] = odomAftMapped->pose.pose.position.x; - transformAftMapped[4] = odomAftMapped->pose.pose.position.y; - transformAftMapped[5] = odomAftMapped->pose.pose.position.z; - - transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; - transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; - transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; - - transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; - transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; - transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "transformMaintenance"); - ros::NodeHandle nh; - - ros::Subscriber subLaserOdometry = nh.subscribe - ("/laser_odom_to_init", 5, laserOdometryHandler); - - ros::Subscriber subOdomAftMapped = nh.subscribe - ("/aft_mapped_to_init", 5, odomAftMappedHandler); - - ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); - pubLaserOdometry2Pointer = &pubLaserOdometry2; - laserOdometry2.header.frame_id = "/camera_init"; - laserOdometry2.child_frame_id = "/camera"; - - tf::TransformBroadcaster tfBroadcaster2; - tfBroadcaster2Pointer = &tfBroadcaster2; - laserOdometryTrans2.frame_id_ = "/camera_init"; - laserOdometryTrans2.child_frame_id_ = "/camera"; - - ros::spin(); - - return 0; -} diff --git a/src/transform_maintenance_node.cpp b/src/transform_maintenance_node.cpp new file mode 100644 index 0000000..87ab611 --- /dev/null +++ b/src/transform_maintenance_node.cpp @@ -0,0 +1,20 @@ +#include +#include "lib/TransformMaintenance.h" + + +/** Main node entry point. */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "transformMaintenance"); + ros::NodeHandle node; + ros::NodeHandle privateNode("~"); + + loam::TransformMaintenance transMaintenance; + + if (transMaintenance.setup(node, privateNode)) { + // initialization successful + ros::spin(); + } + + return 0; +} From c44f0be331125666650567c10d9fd54fb91498bd Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sat, 2 Dec 2017 16:16:41 +0100 Subject: [PATCH 4/9] Removed active/passive modi again... ...as they may be confusing and it's not clear yet how the potential offline version is supposed to work. --- src/lib/LaserOdometry.cpp | 60 ++++++++++++-------------------- src/lib/LaserOdometry.h | 8 ++--- src/lib/ScanRegistration.cpp | 21 +---------- src/lib/ScanRegistration.h | 18 +++------- src/lib/TransformMaintenance.cpp | 9 +---- src/lib/TransformMaintenance.h | 4 +-- 6 files changed, 34 insertions(+), 86 deletions(-) diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp index 246ed9d..2455b15 100644 --- a/src/lib/LaserOdometry.cpp +++ b/src/lib/LaserOdometry.cpp @@ -104,9 +104,6 @@ bool LaserOdometry::setup(ros::NodeHandle &node, _subImuTrans = node.subscribe ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this); - // set active mode - _activeMode = true; - return true; } @@ -344,10 +341,6 @@ void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuT void LaserOdometry::spin() { - if (!_activeMode) { - return; - } - ros::Rate rate(100); bool status = ros::ok(); @@ -819,9 +812,7 @@ void LaserOdometry::process() _laserOdometry.pose.pose.position.x = trans.x(); _laserOdometry.pose.pose.position.y = trans.y(); _laserOdometry.pose.pose.position.z = trans.z(); - if (_activeMode) { - _pubLaserOdometry.publish(_laserOdometry); - } + _pubLaserOdometry.publish(_laserOdometry); _laserOdometryTrans.stamp_ = ros::Time().fromSec(_timeSurfPointsLessFlat); _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); @@ -849,36 +840,31 @@ void LaserOdometry::process() void LaserOdometry::publishResult() { - // only publish messages in active mode - if (!_activeMode) { - // transform each full resolution cloud in passive mode as we don't know who will be using it - transformToEnd(_laserCloud); + // publish results according to the input output ratio + if (_ioRatio > 1 && _frameCount % _ioRatio != 1) { return; } - // publish results according to the input output ratio - if (_ioRatio < 2 || _frameCount % _ioRatio == 1) { - ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); - - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = sweepTime; - laserCloudCornerLast2.header.frame_id = "/camera"; - _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = sweepTime; - laserCloudSurfLast2.header.frame_id = "/camera"; - _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - sensor_msgs::PointCloud2 laserCloudFullRes3; - transformToEnd(_laserCloud); // transform full resolution cloud before sending it - pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = sweepTime; - laserCloudFullRes3.header.frame_id = "/camera"; - _pubLaserCloudFullRes.publish(laserCloudFullRes3); - } + ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); + + sensor_msgs::PointCloud2 laserCloudCornerLast2; + pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); + laserCloudCornerLast2.header.stamp = sweepTime; + laserCloudCornerLast2.header.frame_id = "/camera"; + _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); + + sensor_msgs::PointCloud2 laserCloudSurfLast2; + pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); + laserCloudSurfLast2.header.stamp = sweepTime; + laserCloudSurfLast2.header.frame_id = "/camera"; + _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); + + sensor_msgs::PointCloud2 laserCloudFullRes3; + transformToEnd(_laserCloud); // transform full resolution cloud before sending it + pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = sweepTime; + laserCloudFullRes3.header.frame_id = "/camera"; + _pubLaserCloudFullRes.publish(laserCloudFullRes3); } } // end namespace loam diff --git a/src/lib/LaserOdometry.h b/src/lib/LaserOdometry.h index 8bca58f..c589525 100644 --- a/src/lib/LaserOdometry.h +++ b/src/lib/LaserOdometry.h @@ -52,10 +52,10 @@ namespace loam { */ class LaserOdometry { public: - LaserOdometry(const float& scanPeriod, - const uint16_t& ioRatio = 2); + explicit LaserOdometry(const float& scanPeriod, + const uint16_t& ioRatio = 2); - /** \brief Setup component in active mode. + /** \brief Setup component. * * @param node the ROS node handle * @param privateNode the private ROS node handle @@ -196,8 +196,6 @@ class LaserOdometry { Vector3 _imuShiftFromStart; Vector3 _imuVeloFromStart; - bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) - ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index 3aeff13..c70d4ad 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -59,8 +59,7 @@ ScanRegistration::ScanRegistration(const float& scanPeriod, _regionCurvature(), _regionLabel(), _regionSortIndices(), - _scanNeighborPicked(), - _activeMode(false) + _scanNeighborPicked() { _scanStartIndices.assign(nScans, 0); _scanEndIndices.assign(nScans, 0); @@ -88,19 +87,6 @@ bool ScanRegistration::setup(ros::NodeHandle& node, _pubSurfPointsLessFlat = node.advertise("/laser_cloud_less_flat", 2); _pubImuTrans = node.advertise("/imu_trans", 5); - // set active mode - _activeMode = true; - - return true; -} - - - -bool ScanRegistration::setup(const RegistrationParams& config) -{ - _config = config; - _config.print(); - return true; } @@ -478,11 +464,6 @@ void ScanRegistration::generateROSMsg(sensor_msgs::PointCloud2& msg, void ScanRegistration::publishResult() { - if (!_activeMode) { - // only publish messages in active mode - return; - } - // publish full resolution and feature point clouds sensor_msgs::PointCloud2 laserCloudOutMsg; generateROSMsg(laserCloudOutMsg, _laserCloud); diff --git a/src/lib/ScanRegistration.h b/src/lib/ScanRegistration.h index 008d418..b75ffd2 100644 --- a/src/lib/ScanRegistration.h +++ b/src/lib/ScanRegistration.h @@ -254,12 +254,12 @@ typedef struct IMUState { */ class ScanRegistration { public: - ScanRegistration(const float& scanPeriod, - const uint16_t& nScans = 0, - const size_t& imuHistorySize = 200, - const RegistrationParams& config = RegistrationParams()); + explicit ScanRegistration(const float& scanPeriod, + const uint16_t& nScans = 0, + const size_t& imuHistorySize = 200, + const RegistrationParams& config = RegistrationParams()); - /** \brief Setup component in active mode. + /** \brief Setup component. * * @param node the ROS node handle * @param privateNode the private ROS node handle @@ -267,12 +267,6 @@ class ScanRegistration { virtual bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode); - /** \brief Setup component in passive mode. - * - * @param config the scan registration configuration parameters - */ - virtual bool setup(const RegistrationParams& config); - /** \brief Handler method for IMU messages. * * @param imuIn the new IMU message @@ -393,8 +387,6 @@ class ScanRegistration { std::vector _regionSortIndices; ///< sorted region indices based on point curvature std::vector _scanNeighborPicked; ///< flag if neighboring point was already picked - bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) - ros::Subscriber _subImu; ///< IMU message subscriber ros::Publisher _pubLaserCloud; ///< full resolution cloud message publisher diff --git a/src/lib/TransformMaintenance.cpp b/src/lib/TransformMaintenance.cpp index 0eca7bb..277ad5f 100644 --- a/src/lib/TransformMaintenance.cpp +++ b/src/lib/TransformMaintenance.cpp @@ -67,9 +67,6 @@ bool TransformMaintenance::setup(ros::NodeHandle &node, ros::NodeHandle &private _subOdomAftMapped = node.subscribe ("/aft_mapped_to_init", 5, &TransformMaintenance::odomAftMappedHandler, this); - // set active mode - _activeMode = true; - return true; } @@ -201,11 +198,7 @@ void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstP _laserOdometry2.pose.pose.position.x = _transformMapped[3]; _laserOdometry2.pose.pose.position.y = _transformMapped[4]; _laserOdometry2.pose.pose.position.z = _transformMapped[5]; - - if (_activeMode) { - // only publish messages in active mode - _pubLaserOdometry2.publish(_laserOdometry2); - } + _pubLaserOdometry2.publish(_laserOdometry2); _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); diff --git a/src/lib/TransformMaintenance.h b/src/lib/TransformMaintenance.h index 7ffa85a..472277d 100644 --- a/src/lib/TransformMaintenance.h +++ b/src/lib/TransformMaintenance.h @@ -47,7 +47,7 @@ class TransformMaintenance { public: TransformMaintenance(); - /** \brief Setup component in active mode. + /** \brief Setup component. * * @param node the ROS node handle * @param privateNode the private ROS node handle @@ -87,8 +87,6 @@ class TransformMaintenance { ros::Subscriber _subLaserOdometry; ///< (high frequency) laser odometry subscriber ros::Subscriber _subOdomAftMapped; ///< (low frequency) mapping odometry subscriber - - bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) }; } // end namespace loam From f7457e75f7081628c75236e68d5c7e5f436832b3 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sat, 2 Dec 2017 16:44:33 +0100 Subject: [PATCH 5/9] Extracted laser mapping component into own class. * Moved code for pose optimization into own function. * Reduced scopes of variables where possible. No changes to the logic. This commit completes the extraction of the code into an object oriented library. --- .gitignore | 12 +- CMakeLists.txt | 4 +- src/laserMapping.cpp | 1059 ---------------------------------- src/laser_mapping_node.cpp | 20 + src/lib/CMakeLists.txt | 2 + src/lib/LaserMapping.cpp | 1111 ++++++++++++++++++++++++++++++++++++ src/lib/LaserMapping.h | 232 ++++++++ 7 files changed, 1373 insertions(+), 1067 deletions(-) delete mode 100755 src/laserMapping.cpp create mode 100644 src/laser_mapping_node.cpp create mode 100644 src/lib/LaserMapping.cpp create mode 100644 src/lib/LaserMapping.h diff --git a/.gitignore b/.gitignore index 1da4aac..79a4603 100755 --- a/.gitignore +++ b/.gitignore @@ -1,11 +1,11 @@ bin build -catkin -devel -catkin_generated +catkin +devel +catkin_generated gtest test_results -CMakeFiles -cmake_install.cmake +CMakeFiles +cmake_install.cmake Makefile - +.idea diff --git a/CMakeLists.txt b/CMakeLists.txt index a1298ce..2c1c5ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,8 +37,8 @@ target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam add_executable(laserOdometry src/laser_odometry_node.cpp) target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) -add_executable(laserMapping src/laserMapping.cpp) -target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) +add_executable(laserMapping src/laser_mapping_node.cpp) +target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) add_executable(transformMaintenance src/transform_maintenance_node.cpp) target_link_libraries(transformMaintenance ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp deleted file mode 100755 index d2ba4f9..0000000 --- a/src/laserMapping.cpp +++ /dev/null @@ -1,1059 +0,0 @@ -// Copyright 2013, Ji Zhang, Carnegie Mellon University -// Further contributions copyright (c) 2016, Southwest Research Institute -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This is an implementation of the algorithm described in the following paper: -// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. -// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. - -#include - -#include "loam_velodyne/common.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "loam_velodyne/nanoflann_pcl.h" -#include "lib/math_utils.h" - -using namespace loam; - -const float scanPeriod = 0.1; - -const int stackFrameNum = 1; -const int mapFrameNum = 5; - -double timeLaserCloudCornerLast = 0; -double timeLaserCloudSurfLast = 0; -double timeLaserCloudFullRes = 0; -double timeLaserOdometry = 0; - -bool newLaserCloudCornerLast = false; -bool newLaserCloudSurfLast = false; -bool newLaserCloudFullRes = false; -bool newLaserOdometry = false; - -int laserCloudCenWidth = 10; -int laserCloudCenHeight = 5; -int laserCloudCenDepth = 10; -const int laserCloudWidth = 21; -const int laserCloudHeight = 11; -const int laserCloudDepth = 21; -const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; - -int laserCloudValidInd[125]; -int laserCloudSurroundInd[125]; - -pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerStack2(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfStack2(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudOri(new pcl::PointCloud()); -pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurround2(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum]; -pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum]; -pcl::PointCloud::Ptr laserCloudCornerArray2[laserCloudNum]; -pcl::PointCloud::Ptr laserCloudSurfArray2[laserCloudNum]; - -Twist transformSum; -Twist transformIncre; -Twist transformTobeMapped; -Twist transformBefMapped; -Twist transformAftMapped; - -int imuPointerFront = 0; -int imuPointerLast = -1; -const int imuQueLength = 200; - -double imuTime[imuQueLength] = {0}; -float imuRoll[imuQueLength] = {0}; -float imuPitch[imuQueLength] = {0}; - -void transformAssociateToMap() -{ - transformIncre.pos = transformBefMapped.pos - transformSum.pos; - rotateYXZ(transformIncre.pos, -(transformSum.rot_y), -(transformSum.rot_x), -(transformSum.rot_z)); - - float sbcx = transformSum.rot_x.sin(); - float cbcx = transformSum.rot_x.cos(); - float sbcy = transformSum.rot_y.sin(); - float cbcy = transformSum.rot_y.cos(); - float sbcz = transformSum.rot_z.sin(); - float cbcz = transformSum.rot_z.cos(); - - float sblx = transformBefMapped.rot_x.sin(); - float cblx = transformBefMapped.rot_x.cos(); - float sbly = transformBefMapped.rot_y.sin(); - float cbly = transformBefMapped.rot_y.cos(); - float sblz = transformBefMapped.rot_z.sin(); - float cblz = transformBefMapped.rot_z.cos(); - - float salx = transformAftMapped.rot_x.sin(); - float calx = transformAftMapped.rot_x.cos(); - float saly = transformAftMapped.rot_y.sin(); - float caly = transformAftMapped.rot_y.cos(); - float salz = transformAftMapped.rot_z.sin(); - float calz = transformAftMapped.rot_z.cos(); - - float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) - - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); - transformTobeMapped.rot_x = -asin(srx); - - float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) - - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) - - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) - + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) - + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) - + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); - float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) - - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) - + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) - + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) - - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) - + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); - transformTobeMapped.rot_y = atan2(srycrx / transformTobeMapped.rot_x.cos(), - crycrx / transformTobeMapped.rot_x.cos()); - - float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) - - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); - float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) - - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) - - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) - + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); - transformTobeMapped.rot_z = atan2(srzcrx / transformTobeMapped.rot_x.cos(), - crzcrx / transformTobeMapped.rot_x.cos()); - - Vector3 v = transformIncre.pos; - rotateZXY(v, transformTobeMapped.rot_z, transformTobeMapped.rot_x, transformTobeMapped.rot_y); - transformTobeMapped.pos = transformAftMapped.pos - v; -} - -void transformUpdate() -{ - if (imuPointerLast >= 0) { - float imuRollLast = 0, imuPitchLast = 0; - while (imuPointerFront != imuPointerLast) { - if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) { - break; - } - imuPointerFront = (imuPointerFront + 1) % imuQueLength; - } - - if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) { - imuRollLast = imuRoll[imuPointerFront]; - imuPitchLast = imuPitch[imuPointerFront]; - } else { - int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; - float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) - / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); - float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) - / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); - - imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; - imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; - } - - transformTobeMapped.rot_x = 0.998 * transformTobeMapped.rot_x.rad() + 0.002 * imuPitchLast; - transformTobeMapped.rot_z = 0.998 * transformTobeMapped.rot_z.rad() + 0.002 * imuRollLast; - } - - transformBefMapped = transformSum; - transformAftMapped = transformTobeMapped; -} - -void pointAssociateToMap(PointType const * const pi, PointType * const po) -{ - po->x = pi->x; - po->y = pi->y; - po->z = pi->z; - po->intensity = pi->intensity; - - rotateZXY(*po, transformTobeMapped.rot_z, transformTobeMapped.rot_x, transformTobeMapped.rot_y); - - po->x += transformTobeMapped.pos.x(); - po->y += transformTobeMapped.pos.y(); - po->z += transformTobeMapped.pos.z(); -} - -void pointAssociateTobeMapped(PointType const * const pi, PointType * const po) -{ - po->x = pi->x - transformTobeMapped.pos.x(); - po->y = pi->y - transformTobeMapped.pos.y(); - po->z = pi->z - transformTobeMapped.pos.z(); - po->intensity = pi->intensity; - - rotateYXZ(*po, -transformTobeMapped.rot_y, -transformTobeMapped.rot_x, -transformTobeMapped.rot_z); -} - -void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2) -{ - timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec(); - - laserCloudCornerLast->clear(); - pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast); - - newLaserCloudCornerLast = true; -} - -void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2) -{ - timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec(); - - laserCloudSurfLast->clear(); - pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast); - - newLaserCloudSurfLast = true; -} - -void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) -{ - timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); - - laserCloudFullRes->clear(); - pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); - - newLaserCloudFullRes = true; -} - -void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) -{ - timeLaserOdometry = laserOdometry->header.stamp.toSec(); - - double roll, pitch, yaw; - geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; - tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); - - transformSum.rot_x = -pitch; - transformSum.rot_y = -yaw; - transformSum.rot_z = roll; - - transformSum.pos.x() = laserOdometry->pose.pose.position.x; - transformSum.pos.y() = laserOdometry->pose.pose.position.y; - transformSum.pos.z() = laserOdometry->pose.pose.position.z; - - newLaserOdometry = true; -} - -void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) -{ - double roll, pitch, yaw; - tf::Quaternion orientation; - tf::quaternionMsgToTF(imuIn->orientation, orientation); - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - - imuPointerLast = (imuPointerLast + 1) % imuQueLength; - - imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); - imuRoll[imuPointerLast] = roll; - imuPitch[imuPointerLast] = pitch; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "laserMapping"); - ros::NodeHandle nh; - - ros::Subscriber subLaserCloudCornerLast = nh.subscribe - ("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler); - - ros::Subscriber subLaserCloudSurfLast = nh.subscribe - ("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler); - - ros::Subscriber subLaserOdometry = nh.subscribe - ("/laser_odom_to_init", 5, laserOdometryHandler); - - ros::Subscriber subLaserCloudFullRes = nh.subscribe - ("/velodyne_cloud_3", 2, laserCloudFullResHandler); - - ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); - - ros::Publisher pubLaserCloudSurround = nh.advertise - ("/laser_cloud_surround", 1); - - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/velodyne_cloud_registered", 2); - - ros::Publisher pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 5); - nav_msgs::Odometry odomAftMapped; - odomAftMapped.header.frame_id = "/camera_init"; - odomAftMapped.child_frame_id = "/aft_mapped"; - - tf::TransformBroadcaster tfBroadcaster; - tf::StampedTransform aftMappedTrans; - aftMappedTrans.frame_id_ = "/camera_init"; - aftMappedTrans.child_frame_id_ = "/aft_mapped"; - - std::vector pointSearchInd; - std::vector pointSearchSqDis; - - PointType pointOri, pointSel, pointProj, coeff; - - Eigen::Matrix matA0; - Eigen::Matrix matB0; - Eigen::Vector3f matX0; - Eigen::Matrix3f matA1; - Eigen::Matrix matD1; - Eigen::Matrix3f matV1; - - matA0.setZero(); - matB0.setConstant(-1); - matX0.setZero(); - - matA1.setZero(); - matD1.setZero(); - matV1.setZero(); - - bool isDegenerate = false; - Eigen::Matrix matP; - - pcl::VoxelGrid downSizeFilterCorner; - downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2); - - pcl::VoxelGrid downSizeFilterSurf; - downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4); - - pcl::VoxelGrid downSizeFilterMap; - downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6); - - for (int i = 0; i < laserCloudNum; i++) { - laserCloudCornerArray[i].reset(new pcl::PointCloud()); - laserCloudSurfArray[i].reset(new pcl::PointCloud()); - laserCloudCornerArray2[i].reset(new pcl::PointCloud()); - laserCloudSurfArray2[i].reset(new pcl::PointCloud()); - } - - int frameCount = stackFrameNum - 1; - int mapFrameCount = mapFrameNum - 1; - ros::Rate rate(100); - bool status = ros::ok(); - while (status) { - ros::spinOnce(); - - if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry && - fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 && - fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 && - fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) { - newLaserCloudCornerLast = false; - newLaserCloudSurfLast = false; - newLaserCloudFullRes = false; - newLaserOdometry = false; - - frameCount++; - if (frameCount >= stackFrameNum) { - transformAssociateToMap(); - - int laserCloudCornerLastNum = laserCloudCornerLast->points.size(); - for (int i = 0; i < laserCloudCornerLastNum; i++) { - pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel); - laserCloudCornerStack2->push_back(pointSel); - } - - int laserCloudSurfLastNum = laserCloudSurfLast->points.size(); - for (int i = 0; i < laserCloudSurfLastNum; i++) { - pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel); - laserCloudSurfStack2->push_back(pointSel); - } - } - - if (frameCount >= stackFrameNum) { - frameCount = 0; - - PointType pointOnYAxis; - pointOnYAxis.x = 0.0; - pointOnYAxis.y = 10.0; - pointOnYAxis.z = 0.0; - pointAssociateToMap(&pointOnYAxis, &pointOnYAxis); - - int centerCubeI = int((transformTobeMapped.pos.x() + 25.0) / 50.0) + laserCloudCenWidth; - int centerCubeJ = int((transformTobeMapped.pos.y() + 25.0) / 50.0) + laserCloudCenHeight; - int centerCubeK = int((transformTobeMapped.pos.z() + 25.0) / 50.0) + laserCloudCenDepth; - - if (transformTobeMapped.pos.x() + 25.0 < 0) centerCubeI--; - if (transformTobeMapped.pos.y() + 25.0 < 0) centerCubeJ--; - if (transformTobeMapped.pos.z() + 25.0 < 0) centerCubeK--; - - while (centerCubeI < 3) { - for (int j = 0; j < laserCloudHeight; j++) { - for (int k = 0; k < laserCloudDepth; k++) { - int i = laserCloudWidth - 1; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; i >= 1; i--) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeI++; - laserCloudCenWidth++; - } - - while (centerCubeI >= laserCloudWidth - 3) { - for (int j = 0; j < laserCloudHeight; j++) { - for (int k = 0; k < laserCloudDepth; k++) { - int i = 0; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; i < laserCloudWidth - 1; i++) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeI--; - laserCloudCenWidth--; - } - - while (centerCubeJ < 3) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int k = 0; k < laserCloudDepth; k++) { - int j = laserCloudHeight - 1; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; j >= 1; j--) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeJ++; - laserCloudCenHeight++; - } - - while (centerCubeJ >= laserCloudHeight - 3) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int k = 0; k < laserCloudDepth; k++) { - int j = 0; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; j < laserCloudHeight - 1; j++) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeJ--; - laserCloudCenHeight--; - } - - while (centerCubeK < 3) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int j = 0; j < laserCloudHeight; j++) { - int k = laserCloudDepth - 1; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; k >= 1; k--) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeK++; - laserCloudCenDepth++; - } - - while (centerCubeK >= laserCloudDepth - 3) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int j = 0; j < laserCloudHeight; j++) { - int k = 0; - pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - for (; k < laserCloudDepth - 1; k++) { - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)]; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)]; - } - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeCornerPointer; - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = - laserCloudCubeSurfPointer; - laserCloudCubeCornerPointer->clear(); - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeK--; - laserCloudCenDepth--; - } - - int laserCloudValidNum = 0; - int laserCloudSurroundNum = 0; - for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) { - for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) { - for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) { - if (i >= 0 && i < laserCloudWidth && - j >= 0 && j < laserCloudHeight && - k >= 0 && k < laserCloudDepth) { - - float centerX = 50.0 * (i - laserCloudCenWidth); - float centerY = 50.0 * (j - laserCloudCenHeight); - float centerZ = 50.0 * (k - laserCloudCenDepth); - - PointType transform_pos = (pcl::PointXYZI) transformTobeMapped.pos; - - bool isInLaserFOV = false; - for (int ii = -1; ii <= 1; ii += 2) { - for (int jj = -1; jj <= 1; jj += 2) { - for (int kk = -1; kk <= 1; kk += 2) { - PointType corner; - corner.x = centerX + 25.0 * ii; - corner.y = centerY + 25.0 * jj; - corner.z = centerZ + 25.0 * kk; - - float squaredSide1 = calcSquaredDiff(transform_pos, corner); - float squaredSide2 = calcSquaredDiff(pointOnYAxis, corner); - - float check1 = 100.0 + squaredSide1 - squaredSide2 - - 10.0 * sqrt(3.0) * sqrt(squaredSide1); - - float check2 = 100.0 + squaredSide1 - squaredSide2 - + 10.0 * sqrt(3.0) * sqrt(squaredSide1); - - if (check1 < 0 && check2 > 0) { - isInLaserFOV = true; - } - } - } - } - - if (isInLaserFOV) { - laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j - + laserCloudWidth * laserCloudHeight * k; - laserCloudValidNum++; - } - laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j - + laserCloudWidth * laserCloudHeight * k; - laserCloudSurroundNum++; - } - } - } - } - - laserCloudCornerFromMap->clear(); - laserCloudSurfFromMap->clear(); - for (int i = 0; i < laserCloudValidNum; i++) { - *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; - *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; - } - int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size(); - int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size(); - - int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size(); - for (int i = 0; i < laserCloudCornerStackNum2; i++) { - pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]); - } - - int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size(); - for (int i = 0; i < laserCloudSurfStackNum2; i++) { - pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]); - } - - laserCloudCornerStack->clear(); - downSizeFilterCorner.setInputCloud(laserCloudCornerStack2); - downSizeFilterCorner.filter(*laserCloudCornerStack); - int laserCloudCornerStackNum = laserCloudCornerStack->points.size(); - - laserCloudSurfStack->clear(); - downSizeFilterSurf.setInputCloud(laserCloudSurfStack2); - downSizeFilterSurf.filter(*laserCloudSurfStack); - int laserCloudSurfStackNum = laserCloudSurfStack->points.size(); - - laserCloudCornerStack2->clear(); - laserCloudSurfStack2->clear(); - - if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100) { - - nanoflann::KdTreeFLANN kdtreeCornerFromMap; - nanoflann::KdTreeFLANN kdtreeSurfFromMap; - - kdtreeCornerFromMap.setInputCloud(laserCloudCornerFromMap); - kdtreeSurfFromMap.setInputCloud(laserCloudSurfFromMap); - - pointSearchInd.resize(5); - pointSearchSqDis.resize(5); - - for (int iterCount = 0; iterCount < 10; iterCount++) { - laserCloudOri->clear(); - coeffSel->clear(); - - for (int i = 0; i < laserCloudCornerStackNum; i++) { - pointOri = laserCloudCornerStack->points[i]; - pointAssociateToMap(&pointOri, &pointSel); - kdtreeCornerFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis ); - - if (pointSearchSqDis[4] < 1.0) { - Vector3 vc(0,0,0); - - for (int j = 0; j < 5; j++) { - vc.x() += laserCloudCornerFromMap->points[pointSearchInd[j]].x; - vc.y() += laserCloudCornerFromMap->points[pointSearchInd[j]].y; - vc.z() += laserCloudCornerFromMap->points[pointSearchInd[j]].z; - } - vc /= 5.0; - - Eigen::Matrix3f mat_a; - mat_a.setZero(); - - for (int j = 0; j < 5; j++) { - float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - vc.x(); - float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - vc.y(); - float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - vc.z(); - - mat_a(0,0) += ax * ax; - mat_a(0,1) += ax * ay; - mat_a(0,2) += ax * az; - mat_a(1,1) += ay * ay; - mat_a(1,2) += ay * az; - mat_a(2,2) += az * az; - } - matA1 = mat_a / 5.0; - - Eigen::SelfAdjointEigenSolver esolver(matA1); - matD1 = esolver.eigenvalues().real(); - matV1 = esolver.eigenvectors().real(); - - if (matD1(0, 0) > 3 * matD1(0, 1)) { - - float x0 = pointSel.x; - float y0 = pointSel.y; - float z0 = pointSel.z; - float x1 = vc.x() + 0.1 * matV1(0, 0); - float y1 = vc.y() + 0.1 * matV1(0, 1); - float z1 = vc.z() + 0.1 * matV1(0, 2); - float x2 = vc.x() - 0.1 * matV1(0, 0); - float y2 = vc.y() - 0.1 * matV1(0, 1); - float z2 = vc.z() - 0.1 * matV1(0, 2); - - float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) - * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); - - float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); - - float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; - - float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float ld2 = a012 / l12; - - pointProj = pointSel; - pointProj.x -= la * ld2; - pointProj.y -= lb * ld2; - pointProj.z -= lc * ld2; - - float s = 1 - 0.9 * fabs(ld2); - - coeff.x = s * la; - coeff.y = s * lb; - coeff.z = s * lc; - coeff.intensity = s * ld2; - - if (s > 0.1) { - laserCloudOri->push_back(pointOri); - coeffSel->push_back(coeff); - } - } - } - } - - for (int i = 0; i < laserCloudSurfStackNum; i++) { - pointOri = laserCloudSurfStack->points[i]; - pointAssociateToMap(&pointOri, &pointSel); - kdtreeSurfFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis ); - - if (pointSearchSqDis[4] < 1.0) { - for (int j = 0; j < 5; j++) { - matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; - matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; - matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; - } - matX0 = matA0.colPivHouseholderQr().solve(matB0); - - float pa = matX0(0, 0); - float pb = matX0(1, 0); - float pc = matX0(2, 0); - float pd = 1; - - float ps = sqrt(pa * pa + pb * pb + pc * pc); - pa /= ps; - pb /= ps; - pc /= ps; - pd /= ps; - - bool planeValid = true; - for (int j = 0; j < 5; j++) { - if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x + - pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y + - pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) { - planeValid = false; - break; - } - } - - if (planeValid) { - float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; - - pointProj = pointSel; - pointProj.x -= pa * pd2; - pointProj.y -= pb * pd2; - pointProj.z -= pc * pd2; - - float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x - + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); - - coeff.x = s * pa; - coeff.y = s * pb; - coeff.z = s * pc; - coeff.intensity = s * pd2; - - if (s > 0.1) { - laserCloudOri->push_back(pointOri); - coeffSel->push_back(coeff); - } - } - } - } - - float srx = transformTobeMapped.rot_x.sin(); - float crx = transformTobeMapped.rot_x.cos(); - float sry = transformTobeMapped.rot_y.sin(); - float cry = transformTobeMapped.rot_y.cos(); - float srz = transformTobeMapped.rot_z.sin(); - float crz = transformTobeMapped.rot_z.cos(); - - int laserCloudSelNum = laserCloudOri->points.size(); - if (laserCloudSelNum < 50) { - continue; - } - - Eigen::Matrix matA(laserCloudSelNum, 6); - Eigen::Matrix matAt(6, laserCloudSelNum); - Eigen::Matrix matAtA; - Eigen::VectorXf matB(laserCloudSelNum); - Eigen::VectorXf matAtB; - Eigen::VectorXf matX - ; - for (int i = 0; i < laserCloudSelNum; i++) { - pointOri = laserCloudOri->points[i]; - coeff = coeffSel->points[i]; - - float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x - + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y - + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; - - float ary = ((cry*srx*srz - crz*sry)*pointOri.x - + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x - + ((-cry*crz - srx*sry*srz)*pointOri.x - + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; - - float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x - + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y - + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z; - - matA(i, 0) = arx; - matA(i, 1) = ary; - matA(i, 2) = arz; - matA(i, 3) = coeff.x; - matA(i, 4) = coeff.y; - matA(i, 5) = coeff.z; - matB(i, 0) = -coeff.intensity; - } - matAt = matA.transpose(); - matAtA = matAt * matA; - matAtB = matAt * matB; - matX = matAtA.colPivHouseholderQr().solve(matAtB); - - if (iterCount == 0) { - Eigen::Matrix matE; - Eigen::Matrix matV; - Eigen::Matrix matV2; - - Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); - matE = esolver.eigenvalues().real(); - matV = esolver.eigenvectors().real(); - - matV2 = matV; - - isDegenerate = false; - float eignThre[6] = {100, 100, 100, 100, 100, 100}; - for (int i = 5; i >= 0; i--) { - if (matE(0, i) < eignThre[i]) { - for (int j = 0; j < 6; j++) { - matV2(i, j) = 0; - } - isDegenerate = true; - } else { - break; - } - } - matP = matV.inverse() * matV2; - } - - if (isDegenerate) { - Eigen::Matrix matX2(matX); - matX = matP * matX2; - } - - transformTobeMapped.rot_x += matX(0, 0); - transformTobeMapped.rot_y += matX(1, 0); - transformTobeMapped.rot_z += matX(2, 0); - transformTobeMapped.pos.x() += matX(3, 0); - transformTobeMapped.pos.y() += matX(4, 0); - transformTobeMapped.pos.z() += matX(5, 0); - - float deltaR = sqrt( - pow(rad2deg(matX(0, 0)), 2) + - pow(rad2deg(matX(1, 0)), 2) + - pow(rad2deg(matX(2, 0)), 2)); - float deltaT = sqrt( - pow(matX(3, 0) * 100, 2) + - pow(matX(4, 0) * 100, 2) + - pow(matX(5, 0) * 100, 2)); - - if (deltaR < 0.05 && deltaT < 0.05) { - break; - } - } - - transformUpdate(); - } - - for (int i = 0; i < laserCloudCornerStackNum; i++) { - pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel); - - int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; - int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; - int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; - - if (pointSel.x + 25.0 < 0) cubeI--; - if (pointSel.y + 25.0 < 0) cubeJ--; - if (pointSel.z + 25.0 < 0) cubeK--; - - if (cubeI >= 0 && cubeI < laserCloudWidth && - cubeJ >= 0 && cubeJ < laserCloudHeight && - cubeK >= 0 && cubeK < laserCloudDepth) { - int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; - laserCloudCornerArray[cubeInd]->push_back(pointSel); - } - } - - for (int i = 0; i < laserCloudSurfStackNum; i++) { - pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel); - - int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; - int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; - int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; - - if (pointSel.x + 25.0 < 0) cubeI--; - if (pointSel.y + 25.0 < 0) cubeJ--; - if (pointSel.z + 25.0 < 0) cubeK--; - - if (cubeI >= 0 && cubeI < laserCloudWidth && - cubeJ >= 0 && cubeJ < laserCloudHeight && - cubeK >= 0 && cubeK < laserCloudDepth) { - int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; - laserCloudSurfArray[cubeInd]->push_back(pointSel); - } - } - - for (int i = 0; i < laserCloudValidNum; i++) { - int ind = laserCloudValidInd[i]; - - laserCloudCornerArray2[ind]->clear(); - downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]); - downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]); - - laserCloudSurfArray2[ind]->clear(); - downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]); - downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]); - - pcl::PointCloud::Ptr laserCloudTemp = laserCloudCornerArray[ind]; - laserCloudCornerArray[ind] = laserCloudCornerArray2[ind]; - laserCloudCornerArray2[ind] = laserCloudTemp; - - laserCloudTemp = laserCloudSurfArray[ind]; - laserCloudSurfArray[ind] = laserCloudSurfArray2[ind]; - laserCloudSurfArray2[ind] = laserCloudTemp; - } - - mapFrameCount++; - if (mapFrameCount >= mapFrameNum) { - mapFrameCount = 0; - - laserCloudSurround2->clear(); - for (int i = 0; i < laserCloudSurroundNum; i++) { - int ind = laserCloudSurroundInd[i]; - *laserCloudSurround2 += *laserCloudCornerArray[ind]; - *laserCloudSurround2 += *laserCloudSurfArray[ind]; - } - - laserCloudSurround->clear(); - downSizeFilterCorner.setInputCloud(laserCloudSurround2); - downSizeFilterCorner.filter(*laserCloudSurround); - - sensor_msgs::PointCloud2 laserCloudSurround3; - pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3); - laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry); - laserCloudSurround3.header.frame_id = "/camera_init"; - pubLaserCloudSurround.publish(laserCloudSurround3); - } - - int laserCloudFullResNum = laserCloudFullRes->points.size(); - for (int i = 0; i < laserCloudFullResNum; i++) { - pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); - } - - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry); - laserCloudFullRes3.header.frame_id = "/camera_init"; - pubLaserCloudFullRes.publish(laserCloudFullRes3); - - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw - ( transformAftMapped.rot_z.rad(), - -transformAftMapped.rot_x.rad(), - -transformAftMapped.rot_y.rad()); - - odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); - odomAftMapped.pose.pose.orientation.x = -geoQuat.y; - odomAftMapped.pose.pose.orientation.y = -geoQuat.z; - odomAftMapped.pose.pose.orientation.z = geoQuat.x; - odomAftMapped.pose.pose.orientation.w = geoQuat.w; - odomAftMapped.pose.pose.position.x = transformAftMapped.pos.x(); - odomAftMapped.pose.pose.position.y = transformAftMapped.pos.y(); - odomAftMapped.pose.pose.position.z = transformAftMapped.pos.z(); - odomAftMapped.twist.twist.angular.x = transformBefMapped.rot_x.rad(); - odomAftMapped.twist.twist.angular.y = transformBefMapped.rot_y.rad(); - odomAftMapped.twist.twist.angular.z = transformBefMapped.rot_z.rad(); - odomAftMapped.twist.twist.linear.x = transformBefMapped.pos.x(); - odomAftMapped.twist.twist.linear.y = transformBefMapped.pos.y(); - odomAftMapped.twist.twist.linear.z = transformBefMapped.pos.z(); - pubOdomAftMapped.publish(odomAftMapped); - - aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry); - aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped.pos.x(), - transformAftMapped.pos.y(), - transformAftMapped.pos.z())); - tfBroadcaster.sendTransform(aftMappedTrans); - - } - } - - status = ros::ok(); - rate.sleep(); - } - - return 0; -} - diff --git a/src/laser_mapping_node.cpp b/src/laser_mapping_node.cpp new file mode 100644 index 0000000..2fcc3f3 --- /dev/null +++ b/src/laser_mapping_node.cpp @@ -0,0 +1,20 @@ +#include +#include "lib/LaserMapping.h" + + +/** Main node entry point. */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "laserMapping"); + ros::NodeHandle node; + ros::NodeHandle privateNode("~"); + + loam::LaserMapping laserMapping(0.1); + + if (laserMapping.setup(node, privateNode)) { + // initialization successful + laserMapping.spin(); + } + + return 0; +} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 70a256c..a82faa8 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(loam VelodyneScanRegistration.cpp LaserOdometry.h LaserOdometry.cpp + LaserMapping.h + LaserMapping.cpp TransformMaintenance.h TransformMaintenance.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/LaserMapping.cpp b/src/lib/LaserMapping.cpp new file mode 100644 index 0000000..3d4d75d --- /dev/null +++ b/src/lib/LaserMapping.cpp @@ -0,0 +1,1111 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "LaserMapping.h" +#include "loam_velodyne/nanoflann_pcl.h" + +#include +#include +#include + + +namespace loam { + +using std::sqrt; +using std::fabs; +using std::asin; +using std::atan2; +using std::pow; + + +LaserMapping::LaserMapping(const float& scanPeriod) + : _scanPeriod(scanPeriod), + _stackFrameNum(1), + _mapFrameNum(5), + _frameCount(0), + _mapFrameCount(0), + _laserCloudCenWidth(10), + _laserCloudCenHeight(5), + _laserCloudCenDepth(10), + _laserCloudWidth(21), + _laserCloudHeight(11), + _laserCloudDepth(21), + _laserCloudNum(_laserCloudWidth * _laserCloudHeight * _laserCloudDepth), + _laserCloudCornerLast(new pcl::PointCloud()), + _laserCloudSurfLast(new pcl::PointCloud()), + _laserCloudFullRes(new pcl::PointCloud()), + _laserCloudCornerStack(new pcl::PointCloud()), + _laserCloudSurfStack(new pcl::PointCloud()), + _laserCloudCornerStackDS(new pcl::PointCloud()), + _laserCloudSurfStackDS(new pcl::PointCloud()), + _laserCloudSurround(new pcl::PointCloud()), + _laserCloudSurroundDS(new pcl::PointCloud()), + _laserCloudCornerFromMap(new pcl::PointCloud()), + _laserCloudSurfFromMap(new pcl::PointCloud()) +{ + // initialize mapping odometry and odometry tf messages + _odomAftMapped.header.frame_id = "/camera_init"; + _odomAftMapped.child_frame_id = "/aft_mapped"; + + _aftMappedTrans.frame_id_ = "/camera_init"; + _aftMappedTrans.child_frame_id_ = "/aft_mapped"; + + // initialize frame counter + _frameCount = _stackFrameNum - 1; + _mapFrameCount = _mapFrameNum - 1; + + // setup cloud vectors + _laserCloudCornerArray.resize(_laserCloudNum); + _laserCloudSurfArray.resize(_laserCloudNum); + _laserCloudCornerDSArray.resize(_laserCloudNum); + _laserCloudSurfDSArray.resize(_laserCloudNum); + + for (int i = 0; i < _laserCloudNum; i++) { + _laserCloudCornerArray[i].reset(new pcl::PointCloud()); + _laserCloudSurfArray[i].reset(new pcl::PointCloud()); + _laserCloudCornerDSArray[i].reset(new pcl::PointCloud()); + _laserCloudSurfDSArray[i].reset(new pcl::PointCloud()); + } + + // setup down size filters + _downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2); + _downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4); + _downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6); +} + + + +bool LaserMapping::setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode) +{ + // advertise laser mapping topics + _pubLaserCloudSurround = node.advertise ("/laser_cloud_surround", 1); + _pubLaserCloudFullRes = node.advertise ("/velodyne_cloud_registered", 2); + _pubOdomAftMapped = node.advertise ("/aft_mapped_to_init", 5); + + // subscribe to laser odometry topics + _subLaserCloudCornerLast = node.subscribe + ("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this); + + _subLaserCloudSurfLast = node.subscribe + ("/laser_cloud_surf_last", 2, &LaserMapping::laserCloudSurfLastHandler, this); + + _subLaserOdometry = node.subscribe + ("/laser_odom_to_init", 5, &LaserMapping::laserOdometryHandler, this); + + _subLaserCloudFullRes = node.subscribe + ("/velodyne_cloud_3", 2, &LaserMapping::laserCloudFullResHandler, this); + + // subscribe to IMU topic + _subImu = node.subscribe ("/imu/data", 50, &LaserMapping::imuHandler, this); + + return true; +} + + + +void LaserMapping::transformAssociateToMap() +{ + _transformIncre.pos = _transformBefMapped.pos - _transformSum.pos; + rotateYXZ(_transformIncre.pos, -(_transformSum.rot_y), -(_transformSum.rot_x), -(_transformSum.rot_z)); + + float sbcx = _transformSum.rot_x.sin(); + float cbcx = _transformSum.rot_x.cos(); + float sbcy = _transformSum.rot_y.sin(); + float cbcy = _transformSum.rot_y.cos(); + float sbcz = _transformSum.rot_z.sin(); + float cbcz = _transformSum.rot_z.cos(); + + float sblx = _transformBefMapped.rot_x.sin(); + float cblx = _transformBefMapped.rot_x.cos(); + float sbly = _transformBefMapped.rot_y.sin(); + float cbly = _transformBefMapped.rot_y.cos(); + float sblz = _transformBefMapped.rot_z.sin(); + float cblz = _transformBefMapped.rot_z.cos(); + + float salx = _transformAftMapped.rot_x.sin(); + float calx = _transformAftMapped.rot_x.cos(); + float saly = _transformAftMapped.rot_y.sin(); + float caly = _transformAftMapped.rot_y.cos(); + float salz = _transformAftMapped.rot_z.sin(); + float calz = _transformAftMapped.rot_z.cos(); + + float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) + - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); + _transformTobeMapped.rot_x = -asin(srx); + + float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) + - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) + - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) + + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); + float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) + - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) + + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) + - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); + _transformTobeMapped.rot_y = atan2(srycrx / _transformTobeMapped.rot_x.cos(), + crycrx / _transformTobeMapped.rot_x.cos()); + + float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) + - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) + - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) + + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); + _transformTobeMapped.rot_z = atan2(srzcrx / _transformTobeMapped.rot_x.cos(), + crzcrx / _transformTobeMapped.rot_x.cos()); + + Vector3 v = _transformIncre.pos; + rotateZXY(v, _transformTobeMapped.rot_z, _transformTobeMapped.rot_x, _transformTobeMapped.rot_y); + _transformTobeMapped.pos = _transformAftMapped.pos - v; +} + + + +void LaserMapping::transformUpdate() +{ + if (_imuHistory.size() > 0) { + size_t imuIdx = 0; + + while (imuIdx < _imuHistory.size() - 1 && _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) { + imuIdx++; + } + + IMUState2 imuCur; + + if (imuIdx == 0 || _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) { + // scan time newer then newest or older than oldest IMU message + imuCur = _imuHistory[imuIdx]; + } else { + float ratio = (_imuHistory[imuIdx].stamp.toSec() - _timeLaserOdometry - _scanPeriod) + / (_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp).toSec(); + + IMUState2::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, imuCur); + } + + _transformTobeMapped.rot_x = 0.998 * _transformTobeMapped.rot_x.rad() + 0.002 * imuCur.pitch.rad(); + _transformTobeMapped.rot_z = 0.998 * _transformTobeMapped.rot_z.rad() + 0.002 * imuCur.roll.rad(); + } + + _transformBefMapped = _transformSum; + _transformAftMapped = _transformTobeMapped; +} + + + +void LaserMapping::pointAssociateToMap(const pcl::PointXYZI& pi, pcl::PointXYZI& po) +{ + po.x = pi.x; + po.y = pi.y; + po.z = pi.z; + po.intensity = pi.intensity; + + rotateZXY(po, _transformTobeMapped.rot_z, _transformTobeMapped.rot_x, _transformTobeMapped.rot_y); + + po.x += _transformTobeMapped.pos.x(); + po.y += _transformTobeMapped.pos.y(); + po.z += _transformTobeMapped.pos.z(); +} + + + +void LaserMapping::pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::PointXYZI& po) +{ + po.x = pi.x - _transformTobeMapped.pos.x(); + po.y = pi.y - _transformTobeMapped.pos.y(); + po.z = pi.z - _transformTobeMapped.pos.z(); + po.intensity = pi.intensity; + + rotateYXZ(po, -_transformTobeMapped.rot_y, -_transformTobeMapped.rot_x, -_transformTobeMapped.rot_z); +} + + + +void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg) +{ + _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp.toSec(); + + _laserCloudCornerLast->clear(); + pcl::fromROSMsg(*cornerPointsLastMsg, *_laserCloudCornerLast); + + _newLaserCloudCornerLast = true; +} + + + +void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg) +{ + _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp.toSec(); + + _laserCloudSurfLast->clear(); + pcl::fromROSMsg(*surfacePointsLastMsg, *_laserCloudSurfLast); + + _newLaserCloudSurfLast = true; +} + + + +void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) +{ + _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec(); + + _laserCloudFullRes->clear(); + pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloudFullRes); + + _newLaserCloudFullRes = true; +} + + + +void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) +{ + _timeLaserOdometry = laserOdometry->header.stamp.toSec(); + + double roll, pitch, yaw; + geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); + + _transformSum.rot_x = -pitch; + _transformSum.rot_y = -yaw; + _transformSum.rot_z = roll; + + _transformSum.pos.x() = float(laserOdometry->pose.pose.position.x); + _transformSum.pos.y() = float(laserOdometry->pose.pose.position.y); + _transformSum.pos.z() = float(laserOdometry->pose.pose.position.z); + + _newLaserOdometry = true; +} + + + +void LaserMapping::imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) +{ + double roll, pitch, yaw; + tf::Quaternion orientation; + tf::quaternionMsgToTF(imuIn->orientation, orientation); + tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + + IMUState2 newState; + + newState.stamp = imuIn->header.stamp; + newState.roll = roll; + newState.pitch = pitch; + + _imuHistory.push(newState); +} + + + +void LaserMapping::spin() +{ + ros::Rate rate(100); + bool status = ros::ok(); + + while (status) { + ros::spinOnce(); + + // try processing buffered data + process(); + + status = ros::ok(); + rate.sleep(); + } +} + + +void LaserMapping::reset() +{ + _newLaserCloudCornerLast = false; + _newLaserCloudSurfLast = false; + _newLaserCloudFullRes = false; + _newLaserOdometry = false; +} + + + +bool LaserMapping::hasNewData() +{ + return _newLaserCloudCornerLast && _newLaserCloudSurfLast && + _newLaserCloudFullRes && _newLaserOdometry && + fabs(_timeLaserCloudCornerLast - _timeLaserOdometry) < 0.005 && + fabs(_timeLaserCloudSurfLast - _timeLaserOdometry) < 0.005 && + fabs(_timeLaserCloudFullRes - _timeLaserOdometry) < 0.005; +} + + + +void LaserMapping::process() +{ + if (!hasNewData()) { + // waiting for new data to arrive... + return; + } + + // reset flags, etc. + reset(); + + // skip some frames?!? + _frameCount++; + if (_frameCount < _stackFrameNum) { + return; + } + _frameCount = 0; + + pcl::PointXYZI pointSel; + + // relate incoming data to map + transformAssociateToMap(); + + size_t laserCloudCornerLastNum = _laserCloudCornerLast->points.size(); + for (int i = 0; i < laserCloudCornerLastNum; i++) { + pointAssociateToMap(_laserCloudCornerLast->points[i], pointSel); + _laserCloudCornerStack->push_back(pointSel); + } + + size_t laserCloudSurfLastNum = _laserCloudSurfLast->points.size(); + for (int i = 0; i < laserCloudSurfLastNum; i++) { + pointAssociateToMap(_laserCloudSurfLast->points[i], pointSel); + _laserCloudSurfStack->push_back(pointSel); + } + + + pcl::PointXYZI pointOnYAxis; + pointOnYAxis.x = 0.0; + pointOnYAxis.y = 10.0; + pointOnYAxis.z = 0.0; + pointAssociateToMap(pointOnYAxis, pointOnYAxis); + + int centerCubeI = int((_transformTobeMapped.pos.x() + 25.0) / 50.0) + _laserCloudCenWidth; + int centerCubeJ = int((_transformTobeMapped.pos.y() + 25.0) / 50.0) + _laserCloudCenHeight; + int centerCubeK = int((_transformTobeMapped.pos.z() + 25.0) / 50.0) + _laserCloudCenDepth; + + if (_transformTobeMapped.pos.x() + 25.0 < 0) centerCubeI--; + if (_transformTobeMapped.pos.y() + 25.0 < 0) centerCubeJ--; + if (_transformTobeMapped.pos.z() + 25.0 < 0) centerCubeK--; + + while (centerCubeI < 3) { + for (int j = 0; j < _laserCloudHeight; j++) { + for (int k = 0; k < _laserCloudDepth; k++) { + int i = _laserCloudWidth - 1; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; i >= 1; i--) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i - 1 + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i - 1 + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeI++; + _laserCloudCenWidth++; + } + + while (centerCubeI >= _laserCloudWidth - 3) { + for (int j = 0; j < _laserCloudHeight; j++) { + for (int k = 0; k < _laserCloudDepth; k++) { + int i = 0; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; i < _laserCloudWidth - 1; i++) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i + 1 + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i + 1 + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeI--; + _laserCloudCenWidth--; + } + + while (centerCubeJ < 3) { + for (int i = 0; i < _laserCloudWidth; i++) { + for (int k = 0; k < _laserCloudDepth; k++) { + int j = _laserCloudHeight - 1; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; j >= 1; j--) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i + _laserCloudWidth*(j - 1) + _laserCloudWidth * _laserCloudHeight*k]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i + _laserCloudWidth * (j - 1) + _laserCloudWidth * _laserCloudHeight*k]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeJ++; + _laserCloudCenHeight++; + } + + while (centerCubeJ >= _laserCloudHeight - 3) { + for (int i = 0; i < _laserCloudWidth; i++) { + for (int k = 0; k < _laserCloudDepth; k++) { + int j = 0; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; j < _laserCloudHeight - 1; j++) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i + _laserCloudWidth*(j + 1) + _laserCloudWidth * _laserCloudHeight*k]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i + _laserCloudWidth * (j + 1) + _laserCloudWidth * _laserCloudHeight*k]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeJ--; + _laserCloudCenHeight--; + } + + while (centerCubeK < 3) { + for (int i = 0; i < _laserCloudWidth; i++) { + for (int j = 0; j < _laserCloudHeight; j++) { + int k = _laserCloudDepth - 1; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; k >= 1; k--) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight*(k - 1)]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight*(k - 1)]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeK++; + _laserCloudCenDepth++; + } + + while (centerCubeK >= _laserCloudDepth - 3) { + for (int i = 0; i < _laserCloudWidth; i++) { + for (int j = 0; j < _laserCloudHeight; j++) { + int k = 0; + pcl::PointCloud::Ptr laserCloudCubeCornerPointer = + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + pcl::PointCloud::Ptr laserCloudCubeSurfPointer = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k]; + for (; k < _laserCloudDepth - 1; k++) { + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudCornerArray[i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight*(k + 1)]; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight*(k + 1)]; + } + _laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeCornerPointer; + _laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] = + laserCloudCubeSurfPointer; + laserCloudCubeCornerPointer->clear(); + laserCloudCubeSurfPointer->clear(); + } + } + + centerCubeK--; + _laserCloudCenDepth--; + } + + _laserCloudValidInd.clear(); + _laserCloudSurroundInd.clear(); + for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) { + for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) { + for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) { + if (i >= 0 && i < _laserCloudWidth && + j >= 0 && j < _laserCloudHeight && + k >= 0 && k < _laserCloudDepth) { + + float centerX = 50.0f * (i - _laserCloudCenWidth); + float centerY = 50.0f * (j - _laserCloudCenHeight); + float centerZ = 50.0f * (k - _laserCloudCenDepth); + + pcl::PointXYZI transform_pos = (pcl::PointXYZI) _transformTobeMapped.pos; + + bool isInLaserFOV = false; + for (int ii = -1; ii <= 1; ii += 2) { + for (int jj = -1; jj <= 1; jj += 2) { + for (int kk = -1; kk <= 1; kk += 2) { + pcl::PointXYZI corner; + corner.x = centerX + 25.0f * ii; + corner.y = centerY + 25.0f * jj; + corner.z = centerZ + 25.0f * kk; + + float squaredSide1 = calcSquaredDiff(transform_pos, corner); + float squaredSide2 = calcSquaredDiff(pointOnYAxis, corner); + + float check1 = 100.0f + squaredSide1 - squaredSide2 + - 10.0f * sqrt(3.0f) * sqrt(squaredSide1); + + float check2 = 100.0f + squaredSide1 - squaredSide2 + + 10.0f * sqrt(3.0f) * sqrt(squaredSide1); + + if (check1 < 0 && check2 > 0) { + isInLaserFOV = true; + } + } + } + } + + size_t cubeIdx = i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k; + if (isInLaserFOV) { + _laserCloudValidInd.push_back(cubeIdx); + } + _laserCloudSurroundInd.push_back(cubeIdx); + } + } + } + } + + // prepare valid map corner and surface cloud for pose optimization + _laserCloudCornerFromMap->clear(); + _laserCloudSurfFromMap->clear(); + size_t laserCloudValidNum = _laserCloudValidInd.size(); + for (int i = 0; i < laserCloudValidNum; i++) { + *_laserCloudCornerFromMap += *_laserCloudCornerArray[_laserCloudValidInd[i]]; + *_laserCloudSurfFromMap += *_laserCloudSurfArray[_laserCloudValidInd[i]]; + } + + // prepare feature stack clouds for pose optimization + size_t laserCloudCornerStackNum2 = _laserCloudCornerStack->points.size(); + for (int i = 0; i < laserCloudCornerStackNum2; i++) { + pointAssociateTobeMapped(_laserCloudCornerStack->points[i], _laserCloudCornerStack->points[i]); + } + + size_t laserCloudSurfStackNum2 = _laserCloudSurfStack->points.size(); + for (int i = 0; i < laserCloudSurfStackNum2; i++) { + pointAssociateTobeMapped(_laserCloudSurfStack->points[i], _laserCloudSurfStack->points[i]); + } + + // down sample feature stack clouds + _laserCloudCornerStackDS->clear(); + _downSizeFilterCorner.setInputCloud(_laserCloudCornerStack); + _downSizeFilterCorner.filter(*_laserCloudCornerStackDS); + size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->points.size(); + + _laserCloudSurfStackDS->clear(); + _downSizeFilterSurf.setInputCloud(_laserCloudSurfStack); + _downSizeFilterSurf.filter(*_laserCloudSurfStackDS); + size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->points.size(); + + _laserCloudCornerStack->clear(); + _laserCloudSurfStack->clear(); + + + // run pose optimization + optimizeTransformTobeMapped(); + + + // store down sized corner stack points in corresponding cube clouds + for (int i = 0; i < laserCloudCornerStackNum; i++) { + pointAssociateToMap(_laserCloudCornerStackDS->points[i], pointSel); + + int cubeI = int((pointSel.x + 25.0) / 50.0) + _laserCloudCenWidth; + int cubeJ = int((pointSel.y + 25.0) / 50.0) + _laserCloudCenHeight; + int cubeK = int((pointSel.z + 25.0) / 50.0) + _laserCloudCenDepth; + + if (pointSel.x + 25.0 < 0) cubeI--; + if (pointSel.y + 25.0 < 0) cubeJ--; + if (pointSel.z + 25.0 < 0) cubeK--; + + if (cubeI >= 0 && cubeI < _laserCloudWidth && + cubeJ >= 0 && cubeJ < _laserCloudHeight && + cubeK >= 0 && cubeK < _laserCloudDepth) { + size_t cubeInd = cubeI + _laserCloudWidth * cubeJ + _laserCloudWidth * _laserCloudHeight * cubeK; + _laserCloudCornerArray[cubeInd]->push_back(pointSel); + } + } + + // store down sized surface stack points in corresponding cube clouds + for (int i = 0; i < laserCloudSurfStackNum; i++) { + pointAssociateToMap(_laserCloudSurfStackDS->points[i], pointSel); + + int cubeI = int((pointSel.x + 25.0) / 50.0) + _laserCloudCenWidth; + int cubeJ = int((pointSel.y + 25.0) / 50.0) + _laserCloudCenHeight; + int cubeK = int((pointSel.z + 25.0) / 50.0) + _laserCloudCenDepth; + + if (pointSel.x + 25.0 < 0) cubeI--; + if (pointSel.y + 25.0 < 0) cubeJ--; + if (pointSel.z + 25.0 < 0) cubeK--; + + if (cubeI >= 0 && cubeI < _laserCloudWidth && + cubeJ >= 0 && cubeJ < _laserCloudHeight && + cubeK >= 0 && cubeK < _laserCloudDepth) { + size_t cubeInd = cubeI + _laserCloudWidth * cubeJ + _laserCloudWidth * _laserCloudHeight * cubeK; + _laserCloudSurfArray[cubeInd]->push_back(pointSel); + } + } + + // down size all valid (within field of view) feature cube clouds + for (int i = 0; i < laserCloudValidNum; i++) { + size_t ind = _laserCloudValidInd[i]; + + _laserCloudCornerDSArray[ind]->clear(); + _downSizeFilterCorner.setInputCloud(_laserCloudCornerArray[ind]); + _downSizeFilterCorner.filter(*_laserCloudCornerDSArray[ind]); + + _laserCloudSurfDSArray[ind]->clear(); + _downSizeFilterSurf.setInputCloud(_laserCloudSurfArray[ind]); + _downSizeFilterSurf.filter(*_laserCloudSurfDSArray[ind]); + + // swap cube clouds for next processing + _laserCloudCornerArray[ind].swap(_laserCloudCornerDSArray[ind]); + _laserCloudSurfArray[ind].swap(_laserCloudSurfDSArray[ind]); + } + + + // publish result + publishResult(); +} + + + +void LaserMapping::optimizeTransformTobeMapped() +{ + if (_laserCloudCornerFromMap->points.size() <= 10 || _laserCloudSurfFromMap->points.size() <= 100) { + return; + } + + pcl::PointXYZI pointSel, pointOri, pointProj, coeff; + + std::vector pointSearchInd(5, 0); + std::vector pointSearchSqDis(5, 0); + + nanoflann::KdTreeFLANN kdtreeCornerFromMap; + nanoflann::KdTreeFLANN kdtreeSurfFromMap; + + kdtreeCornerFromMap.setInputCloud(_laserCloudCornerFromMap); + kdtreeSurfFromMap.setInputCloud(_laserCloudSurfFromMap); + + Eigen::Matrix matA0; + Eigen::Matrix matB0; + Eigen::Vector3f matX0; + Eigen::Matrix3f matA1; + Eigen::Matrix matD1; + Eigen::Matrix3f matV1; + + matA0.setZero(); + matB0.setConstant(-1); + matX0.setZero(); + + matA1.setZero(); + matD1.setZero(); + matV1.setZero(); + + bool isDegenerate = false; + Eigen::Matrix matP; + + size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->points.size(); + size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->points.size(); + + pcl::PointCloud laserCloudOri; + pcl::PointCloud coeffSel; + + for (int iterCount = 0; iterCount < 10; iterCount++) { + laserCloudOri.clear(); + coeffSel.clear(); + + for (int i = 0; i < laserCloudCornerStackNum; i++) { + pointOri = _laserCloudCornerStackDS->points[i]; + pointAssociateToMap(pointOri, pointSel); + kdtreeCornerFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis ); + + if (pointSearchSqDis[4] < 1.0) { + Vector3 vc(0,0,0); + + for (int j = 0; j < 5; j++) { + vc.x() += _laserCloudCornerFromMap->points[pointSearchInd[j]].x; + vc.y() += _laserCloudCornerFromMap->points[pointSearchInd[j]].y; + vc.z() += _laserCloudCornerFromMap->points[pointSearchInd[j]].z; + } + vc /= 5.0; + + Eigen::Matrix3f mat_a; + mat_a.setZero(); + + for (int j = 0; j < 5; j++) { + float ax = _laserCloudCornerFromMap->points[pointSearchInd[j]].x - vc.x(); + float ay = _laserCloudCornerFromMap->points[pointSearchInd[j]].y - vc.y(); + float az = _laserCloudCornerFromMap->points[pointSearchInd[j]].z - vc.z(); + + mat_a(0,0) += ax * ax; + mat_a(0,1) += ax * ay; + mat_a(0,2) += ax * az; + mat_a(1,1) += ay * ay; + mat_a(1,2) += ay * az; + mat_a(2,2) += az * az; + } + matA1 = mat_a / 5.0; + + Eigen::SelfAdjointEigenSolver esolver(matA1); + matD1 = esolver.eigenvalues().real(); + matV1 = esolver.eigenvectors().real(); + + if (matD1(0, 0) > 3 * matD1(0, 1)) { + + float x0 = pointSel.x; + float y0 = pointSel.y; + float z0 = pointSel.z; + float x1 = vc.x() + 0.1 * matV1(0, 0); + float y1 = vc.y() + 0.1 * matV1(0, 1); + float z1 = vc.z() + 0.1 * matV1(0, 2); + float x2 = vc.x() - 0.1 * matV1(0, 0); + float y2 = vc.y() - 0.1 * matV1(0, 1); + float z2 = vc.z() - 0.1 * matV1(0, 2); + + float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) + * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); + + float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); + + float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; + + float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float ld2 = a012 / l12; + + // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards? + pointProj = pointSel; + pointProj.x -= la * ld2; + pointProj.y -= lb * ld2; + pointProj.z -= lc * ld2; + + float s = 1 - 0.9f * fabs(ld2); + + coeff.x = s * la; + coeff.y = s * lb; + coeff.z = s * lc; + coeff.intensity = s * ld2; + + if (s > 0.1) { + laserCloudOri.push_back(pointOri); + coeffSel.push_back(coeff); + } + } + } + } + + for (int i = 0; i < laserCloudSurfStackNum; i++) { + pointOri = _laserCloudSurfStackDS->points[i]; + pointAssociateToMap(pointOri, pointSel); + kdtreeSurfFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis ); + + if (pointSearchSqDis[4] < 1.0) { + for (int j = 0; j < 5; j++) { + matA0(j, 0) = _laserCloudSurfFromMap->points[pointSearchInd[j]].x; + matA0(j, 1) = _laserCloudSurfFromMap->points[pointSearchInd[j]].y; + matA0(j, 2) = _laserCloudSurfFromMap->points[pointSearchInd[j]].z; + } + matX0 = matA0.colPivHouseholderQr().solve(matB0); + + float pa = matX0(0, 0); + float pb = matX0(1, 0); + float pc = matX0(2, 0); + float pd = 1; + + float ps = sqrt(pa * pa + pb * pb + pc * pc); + pa /= ps; + pb /= ps; + pc /= ps; + pd /= ps; + + bool planeValid = true; + for (int j = 0; j < 5; j++) { + if (fabs(pa * _laserCloudSurfFromMap->points[pointSearchInd[j]].x + + pb * _laserCloudSurfFromMap->points[pointSearchInd[j]].y + + pc * _laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) { + planeValid = false; + break; + } + } + + if (planeValid) { + float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; + + // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards? + pointProj = pointSel; + pointProj.x -= pa * pd2; + pointProj.y -= pb * pd2; + pointProj.z -= pc * pd2; + + float s = 1 - 0.9f * fabs(pd2) / sqrt(calcPointDistance(pointSel)); + + coeff.x = s * pa; + coeff.y = s * pb; + coeff.z = s * pc; + coeff.intensity = s * pd2; + + if (s > 0.1) { + laserCloudOri.push_back(pointOri); + coeffSel.push_back(coeff); + } + } + } + } + + float srx = _transformTobeMapped.rot_x.sin(); + float crx = _transformTobeMapped.rot_x.cos(); + float sry = _transformTobeMapped.rot_y.sin(); + float cry = _transformTobeMapped.rot_y.cos(); + float srz = _transformTobeMapped.rot_z.sin(); + float crz = _transformTobeMapped.rot_z.cos(); + + size_t laserCloudSelNum = laserCloudOri.points.size(); + if (laserCloudSelNum < 50) { + continue; + } + + Eigen::Matrix matA(laserCloudSelNum, 6); + Eigen::Matrix matAt(6, laserCloudSelNum); + Eigen::Matrix matAtA; + Eigen::VectorXf matB(laserCloudSelNum); + Eigen::VectorXf matAtB; + Eigen::VectorXf matX; + + for (int i = 0; i < laserCloudSelNum; i++) { + pointOri = laserCloudOri.points[i]; + coeff = coeffSel.points[i]; + + float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x + + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y + + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; + + float ary = ((cry*srx*srz - crz*sry)*pointOri.x + + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x + + ((-cry*crz - srx*sry*srz)*pointOri.x + + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; + + float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x + + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y + + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z; + + matA(i, 0) = arx; + matA(i, 1) = ary; + matA(i, 2) = arz; + matA(i, 3) = coeff.x; + matA(i, 4) = coeff.y; + matA(i, 5) = coeff.z; + matB(i, 0) = -coeff.intensity; + } + + matAt = matA.transpose(); + matAtA = matAt * matA; + matAtB = matAt * matB; + matX = matAtA.colPivHouseholderQr().solve(matAtB); + + if (iterCount == 0) { + Eigen::Matrix matE; + Eigen::Matrix matV; + Eigen::Matrix matV2; + + Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); + matE = esolver.eigenvalues().real(); + matV = esolver.eigenvectors().real(); + + matV2 = matV; + + isDegenerate = false; + float eignThre[6] = {100, 100, 100, 100, 100, 100}; + for (int i = 5; i >= 0; i--) { + if (matE(0, i) < eignThre[i]) { + for (int j = 0; j < 6; j++) { + matV2(i, j) = 0; + } + isDegenerate = true; + } else { + break; + } + } + matP = matV.inverse() * matV2; + } + + if (isDegenerate) { + Eigen::Matrix matX2(matX); + matX = matP * matX2; + } + + _transformTobeMapped.rot_x += matX(0, 0); + _transformTobeMapped.rot_y += matX(1, 0); + _transformTobeMapped.rot_z += matX(2, 0); + _transformTobeMapped.pos.x() += matX(3, 0); + _transformTobeMapped.pos.y() += matX(4, 0); + _transformTobeMapped.pos.z() += matX(5, 0); + + float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) + + pow(rad2deg(matX(1, 0)), 2) + + pow(rad2deg(matX(2, 0)), 2)); + float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + + pow(matX(4, 0) * 100, 2) + + pow(matX(5, 0) * 100, 2)); + + if (deltaR < 0.05 && deltaT < 0.05) { + break; + } + } + + transformUpdate(); +} + + + +void LaserMapping::publishResult() +{ + // publish new map cloud according to the input output ratio + _mapFrameCount++; + if (_mapFrameCount >= _mapFrameNum) { + _mapFrameCount = 0; + + // accumulate map cloud + _laserCloudSurround->clear(); + size_t laserCloudSurroundNum = _laserCloudSurroundInd.size(); + for (int i = 0; i < laserCloudSurroundNum; i++) { + size_t ind = _laserCloudSurroundInd[i]; + *_laserCloudSurround += *_laserCloudCornerArray[ind]; + *_laserCloudSurround += *_laserCloudSurfArray[ind]; + } + + // down size map cloud + _laserCloudSurroundDS->clear(); + _downSizeFilterCorner.setInputCloud(_laserCloudSurround); + _downSizeFilterCorner.filter(*_laserCloudSurroundDS); + + // publish new map cloud + sensor_msgs::PointCloud2 laserCloudSurround3; + pcl::toROSMsg(*_laserCloudSurroundDS, laserCloudSurround3); + laserCloudSurround3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); + laserCloudSurround3.header.frame_id = "/camera_init"; + _pubLaserCloudSurround.publish(laserCloudSurround3); + } + + + // transform full resolution input cloud to map + size_t laserCloudFullResNum = _laserCloudFullRes->points.size(); + for (int i = 0; i < laserCloudFullResNum; i++) { + pointAssociateToMap(_laserCloudFullRes->points[i], _laserCloudFullRes->points[i]); + } + + // publish transformed full resolution input cloud + sensor_msgs::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*_laserCloudFullRes, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); + laserCloudFullRes3.header.frame_id = "/camera_init"; + _pubLaserCloudFullRes.publish(laserCloudFullRes3); + + + // publish odometry after mapped transformations + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw + ( _transformAftMapped.rot_z.rad(), + -_transformAftMapped.rot_x.rad(), + -_transformAftMapped.rot_y.rad()); + + _odomAftMapped.header.stamp = ros::Time().fromSec(_timeLaserOdometry); + _odomAftMapped.pose.pose.orientation.x = -geoQuat.y; + _odomAftMapped.pose.pose.orientation.y = -geoQuat.z; + _odomAftMapped.pose.pose.orientation.z = geoQuat.x; + _odomAftMapped.pose.pose.orientation.w = geoQuat.w; + _odomAftMapped.pose.pose.position.x = _transformAftMapped.pos.x(); + _odomAftMapped.pose.pose.position.y = _transformAftMapped.pos.y(); + _odomAftMapped.pose.pose.position.z = _transformAftMapped.pos.z(); + _odomAftMapped.twist.twist.angular.x = _transformBefMapped.rot_x.rad(); + _odomAftMapped.twist.twist.angular.y = _transformBefMapped.rot_y.rad(); + _odomAftMapped.twist.twist.angular.z = _transformBefMapped.rot_z.rad(); + _odomAftMapped.twist.twist.linear.x = _transformBefMapped.pos.x(); + _odomAftMapped.twist.twist.linear.y = _transformBefMapped.pos.y(); + _odomAftMapped.twist.twist.linear.z = _transformBefMapped.pos.z(); + _pubOdomAftMapped.publish(_odomAftMapped); + + _aftMappedTrans.stamp_ = ros::Time().fromSec(_timeLaserOdometry); + _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + _aftMappedTrans.setOrigin(tf::Vector3(_transformAftMapped.pos.x(), + _transformAftMapped.pos.y(), + _transformAftMapped.pos.z())); + _tfBroadcaster.sendTransform(_aftMappedTrans); +} + +} // end namespace loam diff --git a/src/lib/LaserMapping.h b/src/lib/LaserMapping.h new file mode 100644 index 0000000..e4c404e --- /dev/null +++ b/src/lib/LaserMapping.h @@ -0,0 +1,232 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_LASERMAPPING_H +#define LOAM_LASERMAPPING_H + +#include "math_utils.h" +#include "CircularBuffer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace loam { + +/** IMU state data. */ +typedef struct IMUState2 { + /** The time of the measurement leading to this state (in seconds). */ + ros::Time stamp; + + /** The current roll angle. */ + Angle roll; + + /** The current pitch angle. */ + Angle pitch; + + /** \brief Interpolate between two IMU states. + * + * @param start the first IMU state + * @param end the second IMU state + * @param ratio the interpolation ratio + * @param result the target IMU state for storing the interpolation result + */ + static void interpolate(const IMUState2& start, + const IMUState2& end, + const float& ratio, + IMUState2& result) + { + float invRatio = 1 - ratio; + + result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio; + result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio; + }; +} IMUState2; + + + +/** \brief Implementation of the LOAM laser mapping component. + * + */ +class LaserMapping { +public: + explicit LaserMapping(const float& scanPeriod); + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + virtual bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Handler method for a new last corner cloud. + * + * @param cornerPointsLastMsg the new last corner cloud message + */ + void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg); + + /** \brief Handler method for a new last surface cloud. + * + * @param surfacePointsLastMsg the new last surface cloud message + */ + void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg); + + /** \brief Handler method for a new full resolution cloud. + * + * @param laserCloudFullResMsg the new full resolution cloud message + */ + void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg); + + /** \brief Handler method for a new laser odometry. + * + * @param laserOdometry the new laser odometry message + */ + void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); + + /** \brief Handler method for IMU messages. + * + * @param imuIn the new IMU message + */ + void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn); + + /** \brief Process incoming messages in a loop until shutdown (used in active mode). */ + void spin(); + + /** \brief Try to process buffered data. */ + void process(); + + +protected: + /** \brief Reset flags, etc. */ + void reset(); + + /** \brief Check if all required information for a new processing step is available. */ + bool hasNewData(); + + /** Run an optimization. */ + void optimizeTransformTobeMapped(); + + void transformAssociateToMap(); + void transformUpdate(); + void pointAssociateToMap(const pcl::PointXYZI& pi, pcl::PointXYZI& po); + void pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::PointXYZI& po); + + /** \brief Publish the current result via the respective topics. */ + void publishResult(); + + +private: + const float _scanPeriod; + const int _stackFrameNum; + const int _mapFrameNum; + long _frameCount; + long _mapFrameCount; + + int _laserCloudCenWidth; + int _laserCloudCenHeight; + int _laserCloudCenDepth; + const size_t _laserCloudWidth; + const size_t _laserCloudHeight; + const size_t _laserCloudDepth; + const size_t _laserCloudNum; + + pcl::PointCloud::Ptr _laserCloudCornerLast; ///< last corner points cloud + pcl::PointCloud::Ptr _laserCloudSurfLast; ///< last surface points cloud + pcl::PointCloud::Ptr _laserCloudFullRes; ///< last full resolution cloud + + pcl::PointCloud::Ptr _laserCloudCornerStack; + pcl::PointCloud::Ptr _laserCloudSurfStack; + pcl::PointCloud::Ptr _laserCloudCornerStackDS; ///< down sampled + pcl::PointCloud::Ptr _laserCloudSurfStackDS; ///< down sampled + + pcl::PointCloud::Ptr _laserCloudSurround; + pcl::PointCloud::Ptr _laserCloudSurroundDS; ///< down sampled + pcl::PointCloud::Ptr _laserCloudCornerFromMap; + pcl::PointCloud::Ptr _laserCloudSurfFromMap; + + std::vector::Ptr> _laserCloudCornerArray; + std::vector::Ptr> _laserCloudSurfArray; + std::vector::Ptr> _laserCloudCornerDSArray; ///< down sampled + std::vector::Ptr> _laserCloudSurfDSArray; ///< down sampled + + std::vector _laserCloudValidInd; + std::vector _laserCloudSurroundInd; + + double _timeLaserCloudCornerLast; ///< time of current last corner cloud + double _timeLaserCloudSurfLast; ///< time of current last surface cloud + double _timeLaserCloudFullRes; ///< time of current full resolution cloud + double _timeLaserOdometry; ///< time of current laser odometry + + bool _newLaserCloudCornerLast; ///< flag if a new last corner cloud has been received + bool _newLaserCloudSurfLast; ///< flag if a new last surface cloud has been received + bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received + bool _newLaserOdometry; ///< flag if a new laser odometry has been received + + Twist _transformSum; + Twist _transformIncre; + Twist _transformTobeMapped; + Twist _transformBefMapped; + Twist _transformAftMapped; + + CircularBuffer _imuHistory; ///< history of IMU states + + pcl::VoxelGrid _downSizeFilterCorner; ///< voxel filter for down sizing corner clouds + pcl::VoxelGrid _downSizeFilterSurf; ///< voxel filter for down sizing surface clouds + pcl::VoxelGrid _downSizeFilterMap; ///< voxel filter for down sizing accumulated map + + nav_msgs::Odometry _odomAftMapped; ///< mapping odometry message + tf::StampedTransform _aftMappedTrans; ///< mapping odometry transformation + + ros::Publisher _pubLaserCloudSurround; ///< map cloud message publisher + ros::Publisher _pubLaserCloudFullRes; ///< current full resolution cloud message publisher + ros::Publisher _pubOdomAftMapped; ///< mapping odometry publisher + tf::TransformBroadcaster _tfBroadcaster; ///< mapping odometry transform broadcaster + + ros::Subscriber _subLaserCloudCornerLast; ///< last corner cloud message subscriber + ros::Subscriber _subLaserCloudSurfLast; ///< last surface cloud message subscriber + ros::Subscriber _subLaserCloudFullRes; ///< full resolution cloud message subscriber + ros::Subscriber _subLaserOdometry; ///< laser odometry message subscriber + ros::Subscriber _subImu; ///< IMU message subscriber + +}; + +} // end namespace loam + +#endif //LOAM_LASERMAPPING_H From aa9965d0f5b208561b0ef788c4efa6d96e30b3c1 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sun, 3 Dec 2017 12:50:44 +0100 Subject: [PATCH 6/9] Code and project cleanup. * Moved all class header files into include directory. * Moved Vector3, Angle and Twist class definitions from math_utils.h into own loam_types.h in include directory. * Added publish cloud message function to common.h to simplify publishing of cloud messages. * Made all point calculations / transformations in math_utils.h generic with respect to the pcl point type. --- .../loam_velodyne}/CircularBuffer.h | 0 .../loam_velodyne}/LaserMapping.h | 3 +- .../loam_velodyne}/LaserOdometry.h | 3 +- .../loam_velodyne}/ScanRegistration.h | 11 +- .../loam_velodyne}/TransformMaintenance.h | 0 .../loam_velodyne}/VelodyneScanRegistration.h | 4 +- include/loam_velodyne/common.h | 33 +++- include/loam_velodyne/loam_types.h | 140 +++++++++++++++ src/laser_mapping_node.cpp | 2 +- src/laser_odometry_node.cpp | 2 +- src/lib/CMakeLists.txt | 6 - src/lib/LaserMapping.cpp | 23 +-- src/lib/LaserOdometry.cpp | 27 +-- src/lib/ScanRegistration.cpp | 59 ++----- src/lib/TransformMaintenance.cpp | 8 +- src/lib/VelodyneScanRegistration.cpp | 7 +- src/lib/math_utils.h | 162 +++--------------- src/transform_maintenance_node.cpp | 2 +- src/velodyne_scan_registration_node.cpp | 2 +- 19 files changed, 250 insertions(+), 244 deletions(-) rename {src/lib => include/loam_velodyne}/CircularBuffer.h (100%) rename {src/lib => include/loam_velodyne}/LaserMapping.h (99%) rename {src/lib => include/loam_velodyne}/LaserOdometry.h (99%) rename {src/lib => include/loam_velodyne}/ScanRegistration.h (97%) rename {src/lib => include/loam_velodyne}/TransformMaintenance.h (100%) rename {src/lib => include/loam_velodyne}/VelodyneScanRegistration.h (97%) create mode 100644 include/loam_velodyne/loam_types.h diff --git a/src/lib/CircularBuffer.h b/include/loam_velodyne/CircularBuffer.h similarity index 100% rename from src/lib/CircularBuffer.h rename to include/loam_velodyne/CircularBuffer.h diff --git a/src/lib/LaserMapping.h b/include/loam_velodyne/LaserMapping.h similarity index 99% rename from src/lib/LaserMapping.h rename to include/loam_velodyne/LaserMapping.h index e4c404e..d1e8af5 100644 --- a/src/lib/LaserMapping.h +++ b/include/loam_velodyne/LaserMapping.h @@ -33,7 +33,8 @@ #ifndef LOAM_LASERMAPPING_H #define LOAM_LASERMAPPING_H -#include "math_utils.h" + +#include "loam_types.h" #include "CircularBuffer.h" #include diff --git a/src/lib/LaserOdometry.h b/include/loam_velodyne/LaserOdometry.h similarity index 99% rename from src/lib/LaserOdometry.h rename to include/loam_velodyne/LaserOdometry.h index c589525..e77fa15 100644 --- a/src/lib/LaserOdometry.h +++ b/include/loam_velodyne/LaserOdometry.h @@ -33,7 +33,8 @@ #ifndef LOAM_LASERODOMETRY_H #define LOAM_LASERODOMETRY_H -#include "math_utils.h" + +#include "loam_types.h" #include "loam_velodyne/nanoflann_pcl.h" #include diff --git a/src/lib/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h similarity index 97% rename from src/lib/ScanRegistration.h rename to include/loam_velodyne/ScanRegistration.h index b75ffd2..cb194f9 100644 --- a/src/lib/ScanRegistration.h +++ b/include/loam_velodyne/ScanRegistration.h @@ -34,17 +34,14 @@ #define LOAM_SCANREGISTRATION_H +#include "loam_types.h" #include "CircularBuffer.h" -#include "math_utils.h" - -#include #include #include -#include #include -#include #include +#include namespace loam { @@ -353,10 +350,6 @@ class ScanRegistration { */ void setIMUTrans(const double& sweepDuration); - /** \brief Generate a point cloud message for the specified cloud. */ - void generateROSMsg(sensor_msgs::PointCloud2& msg, - const pcl::PointCloud::Ptr& cloud); - /** \brief Publish the current result via the respective topics. */ void publishResult(); diff --git a/src/lib/TransformMaintenance.h b/include/loam_velodyne/TransformMaintenance.h similarity index 100% rename from src/lib/TransformMaintenance.h rename to include/loam_velodyne/TransformMaintenance.h diff --git a/src/lib/VelodyneScanRegistration.h b/include/loam_velodyne/VelodyneScanRegistration.h similarity index 97% rename from src/lib/VelodyneScanRegistration.h rename to include/loam_velodyne/VelodyneScanRegistration.h index f0852e2..cbe474e 100644 --- a/src/lib/VelodyneScanRegistration.h +++ b/include/loam_velodyne/VelodyneScanRegistration.h @@ -34,7 +34,9 @@ #define LOAM_VELODYNESCANREGISTRATION_H -#include "ScanRegistration.h" +#include "loam_velodyne/ScanRegistration.h" + +#include namespace loam { diff --git a/include/loam_velodyne/common.h b/include/loam_velodyne/common.h index d47348c..e4bfa50 100644 --- a/include/loam_velodyne/common.h +++ b/include/loam_velodyne/common.h @@ -30,6 +30,37 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. +#ifndef LOAM_COMMON_H +#define LOAM_COMMON_H + +#include +#include +#include #include -typedef pcl::PointXYZI PointType; + +namespace loam { + +/** \brief Construct a new point cloud message from the specified information and publish it via the given publisher. + * + * @tparam PointT the point type + * @param publisher the publisher instance + * @param cloud the cloud to publish + * @param stamp the time stamp of the cloud message + * @param frameID the message frame ID + */ +template +inline void publishCloudMsg(ros::Publisher& publisher, + const pcl::PointCloud& cloud, + const ros::Time& stamp, + std::string frameID) { + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + msg.header.stamp = stamp; + msg.header.frame_id = frameID; + publisher.publish(msg); +} + +} // end namespace loam + +#endif // LOAM_COMMON_H diff --git a/include/loam_velodyne/loam_types.h b/include/loam_velodyne/loam_types.h new file mode 100644 index 0000000..1afee6c --- /dev/null +++ b/include/loam_velodyne/loam_types.h @@ -0,0 +1,140 @@ +#ifndef LOAM_LOAM_TYPES_H +#define LOAM_LOAM_TYPES_H + + +#include + + +namespace loam { + +/** \brief Vector4f decorator for convenient handling. + * + */ +class Vector3 : public Eigen::Vector4f { +public: + Vector3(float x, float y, float z) + : Eigen::Vector4f(x, y, z, 0) {} + + Vector3(void) + : Eigen::Vector4f(0, 0, 0, 0) {} + + template + Vector3(const Eigen::MatrixBase &other) + : Eigen::Vector4f(other) {} + + Vector3(const pcl::PointXYZI &p) + : Eigen::Vector4f(p.x, p.y, p.z, 0) {} + + template + Vector3 &operator=(const Eigen::MatrixBase &rhs) { + this->Eigen::Vector4f::operator=(rhs); + return *this; + } + + Vector3 &operator=(const pcl::PointXYZ &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + Vector3 &operator=(const pcl::PointXYZI &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + float x() const { return (*this)(0); } + + float y() const { return (*this)(1); } + + float z() const { return (*this)(2); } + + float &x() { return (*this)(0); } + + float &y() { return (*this)(1); } + + float &z() { return (*this)(2); } + + // easy conversion + operator pcl::PointXYZI() { + pcl::PointXYZI dst; + dst.x = x(); + dst.y = y(); + dst.z = z(); + dst.intensity = 0; + return dst; + } +}; + + +/** \brief Class for holding an angle. + * + * This class provides buffered access to sine and cosine values to the represented angular value. + */ +class Angle { +public: + Angle() + : _radian(0.0), + _cos(1.0), + _sin(0.0) {} + + Angle(float radValue) + : _radian(radValue), + _cos(std::cos(radValue)), + _sin(std::sin(radValue)) {} + + Angle(const Angle &other) + : _radian(other._radian), + _cos(other._cos), + _sin(other._sin) {} + + void operator=(const Angle &rhs) { + _radian = (rhs._radian); + _cos = (rhs._cos); + _sin = (rhs._sin); + } + + void operator+=(const float &radValue) { *this = (_radian + radValue); } + + void operator+=(const Angle &other) { *this = (_radian + other._radian); } + + void operator-=(const float &radValue) { *this = (_radian - radValue); } + + void operator-=(const Angle &other) { *this = (_radian - other._radian); } + + Angle operator-() const { + Angle out; + out._radian = -_radian; + out._cos = _cos; + out._sin = -(_sin); + return out; + } + + float rad() const { return _radian; } + + float deg() const { return _radian * 180 / M_PI; } + + float cos() const { return _cos; } + + float sin() const { return _sin; } + +private: + float _radian; ///< angle value in radian + float _cos; ///< cosine of the angle + float _sin; ///< sine of the angle +}; + + +/** \brief Twist composed by three angles and a three-dimensional position. */ +struct Twist { + Angle rot_x; + Angle rot_y; + Angle rot_z; + Vector3 pos; +}; + +} // end namespace loam + +#endif //LOAM_LOAM_TYPES_H diff --git a/src/laser_mapping_node.cpp b/src/laser_mapping_node.cpp index 2fcc3f3..5a34dcc 100644 --- a/src/laser_mapping_node.cpp +++ b/src/laser_mapping_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/LaserMapping.h" +#include "loam_velodyne/LaserMapping.h" /** Main node entry point. */ diff --git a/src/laser_odometry_node.cpp b/src/laser_odometry_node.cpp index b1187bd..e73ff25 100644 --- a/src/laser_odometry_node.cpp +++ b/src/laser_odometry_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/LaserOdometry.h" +#include "loam_velodyne/LaserOdometry.h" /** Main node entry point. */ diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a82faa8..da2f148 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,14 +1,8 @@ add_library(loam math_utils.h - CircularBuffer.h - ScanRegistration.h ScanRegistration.cpp - VelodyneScanRegistration.h VelodyneScanRegistration.cpp - LaserOdometry.h LaserOdometry.cpp - LaserMapping.h LaserMapping.cpp - TransformMaintenance.h TransformMaintenance.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/LaserMapping.cpp b/src/lib/LaserMapping.cpp index 3d4d75d..940f528 100644 --- a/src/lib/LaserMapping.cpp +++ b/src/lib/LaserMapping.cpp @@ -30,10 +30,11 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "LaserMapping.h" +#include "loam_velodyne/LaserMapping.h" +#include "loam_velodyne/common.h" #include "loam_velodyne/nanoflann_pcl.h" +#include "math_utils.h" -#include #include #include @@ -1036,6 +1037,8 @@ void LaserMapping::optimizeTransformTobeMapped() void LaserMapping::publishResult() { + ros::Time odometryTime = ros::Time().fromSec(_timeLaserOdometry); + // publish new map cloud according to the input output ratio _mapFrameCount++; if (_mapFrameCount >= _mapFrameNum) { @@ -1056,11 +1059,7 @@ void LaserMapping::publishResult() _downSizeFilterCorner.filter(*_laserCloudSurroundDS); // publish new map cloud - sensor_msgs::PointCloud2 laserCloudSurround3; - pcl::toROSMsg(*_laserCloudSurroundDS, laserCloudSurround3); - laserCloudSurround3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); - laserCloudSurround3.header.frame_id = "/camera_init"; - _pubLaserCloudSurround.publish(laserCloudSurround3); + publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, odometryTime, "/camera_init"); } @@ -1071,11 +1070,7 @@ void LaserMapping::publishResult() } // publish transformed full resolution input cloud - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*_laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); - laserCloudFullRes3.header.frame_id = "/camera_init"; - _pubLaserCloudFullRes.publish(laserCloudFullRes3); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, odometryTime, "/camera_init"); // publish odometry after mapped transformations @@ -1084,7 +1079,7 @@ void LaserMapping::publishResult() -_transformAftMapped.rot_x.rad(), -_transformAftMapped.rot_y.rad()); - _odomAftMapped.header.stamp = ros::Time().fromSec(_timeLaserOdometry); + _odomAftMapped.header.stamp = odometryTime; _odomAftMapped.pose.pose.orientation.x = -geoQuat.y; _odomAftMapped.pose.pose.orientation.y = -geoQuat.z; _odomAftMapped.pose.pose.orientation.z = geoQuat.x; @@ -1100,7 +1095,7 @@ void LaserMapping::publishResult() _odomAftMapped.twist.twist.linear.z = _transformBefMapped.pos.z(); _pubOdomAftMapped.publish(_odomAftMapped); - _aftMappedTrans.stamp_ = ros::Time().fromSec(_timeLaserOdometry); + _aftMappedTrans.stamp_ = odometryTime; _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); _aftMappedTrans.setOrigin(tf::Vector3(_transformAftMapped.pos.x(), _transformAftMapped.pos.y(), diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp index 2455b15..172882a 100644 --- a/src/lib/LaserOdometry.cpp +++ b/src/lib/LaserOdometry.cpp @@ -30,9 +30,10 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "LaserOdometry.h" +#include "loam_velodyne/LaserOdometry.h" +#include "loam_velodyne/common.h" +#include "math_utils.h" -#include #include #include #include @@ -846,25 +847,11 @@ void LaserOdometry::publishResult() } ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); + transformToEnd(_laserCloud); // transform full resolution cloud to sweep end before sending it - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = sweepTime; - laserCloudCornerLast2.header.frame_id = "/camera"; - _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = sweepTime; - laserCloudSurfLast2.header.frame_id = "/camera"; - _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - sensor_msgs::PointCloud2 laserCloudFullRes3; - transformToEnd(_laserCloud); // transform full resolution cloud before sending it - pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = sweepTime; - laserCloudFullRes3.header.frame_id = "/camera"; - _pubLaserCloudFullRes.publish(laserCloudFullRes3); + publishCloudMsg(_pubLaserCloudCornerLast, *_lastCornerCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudSurfLast, *_lastSurfaceCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloud, sweepTime, "/camera"); } } // end namespace loam diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index c70d4ad..166742e 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -30,7 +30,9 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "ScanRegistration.h" +#include "loam_velodyne/ScanRegistration.h" +#include "loam_velodyne/common.h" +#include "math_utils.h" #include #include @@ -408,13 +410,13 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, // mark unreliable points as picked for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) { - const PointType& prevPoint = (_laserCloud->points[i - 1]); - const PointType& point = (_laserCloud->points[i]); - const PointType& nextPoint = (_laserCloud->points[i + 1]); + const pcl::PointXYZI& previousPoint = (_laserCloud->points[i - 1]); + const pcl::PointXYZI& point = (_laserCloud->points[i]); + const pcl::PointXYZI& nextPoint = (_laserCloud->points[i + 1]); - float diff = calcSquaredDiff(nextPoint, point); + float diffNext = calcSquaredDiff(nextPoint, point); - if (diff > 0.1) { + if (diffNext > 0.1) { float depth1 = calcPointDistance(point); float depth2 = calcPointDistance(nextPoint); @@ -441,10 +443,10 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, } } - float diff2 = calcSquaredDiff(point, prevPoint); + float diffPrevious = calcSquaredDiff(point, previousPoint); float dis = calcSquaredPointDistance(point); - if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { + if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) { _scanNeighborPicked[i - startIdx] = 1; } } @@ -452,46 +454,17 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, -void ScanRegistration::generateROSMsg(sensor_msgs::PointCloud2& msg, - const pcl::PointCloud::Ptr& cloud) -{ - pcl::toROSMsg(*cloud, msg); - msg.header.stamp = _sweepStamp; - msg.header.frame_id = "/camera"; -} - - - void ScanRegistration::publishResult() { // publish full resolution and feature point clouds - sensor_msgs::PointCloud2 laserCloudOutMsg; - generateROSMsg(laserCloudOutMsg, _laserCloud); - _pubLaserCloud.publish(laserCloudOutMsg); - - sensor_msgs::PointCloud2 cornerPointsSharpMsg; - generateROSMsg(cornerPointsSharpMsg, _cornerPointsSharp); - _pubCornerPointsSharp.publish(cornerPointsSharpMsg); - - sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; - generateROSMsg(cornerPointsLessSharpMsg, _cornerPointsLessSharp); - _pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); - - sensor_msgs::PointCloud2 surfPointsFlat; - generateROSMsg(surfPointsFlat, _surfacePointsFlat); - _pubSurfPointsFlat.publish(surfPointsFlat); - - sensor_msgs::PointCloud2 surfPointsLessFlat; - generateROSMsg(surfPointsLessFlat, _surfacePointsLessFlat); - _pubSurfPointsLessFlat.publish(surfPointsLessFlat); - + publishCloudMsg(_pubLaserCloud, *_laserCloud, _sweepStamp, "/camera"); + publishCloudMsg(_pubCornerPointsSharp, *_cornerPointsSharp, _sweepStamp, "/camera"); + publishCloudMsg(_pubCornerPointsLessSharp, *_cornerPointsLessSharp, _sweepStamp, "/camera"); + publishCloudMsg(_pubSurfPointsFlat, *_surfacePointsFlat, _sweepStamp, "/camera"); + publishCloudMsg(_pubSurfPointsLessFlat, *_surfacePointsLessFlat, _sweepStamp, "/camera"); // publish corresponding IMU transformation information - sensor_msgs::PointCloud2 imuTransMsg; - pcl::toROSMsg(*_imuTrans, imuTransMsg); - imuTransMsg.header.stamp = _sweepStamp; - imuTransMsg.header.frame_id = "/camera"; - _pubImuTrans.publish(imuTransMsg); + publishCloudMsg(_pubImuTrans, *_imuTrans, _sweepStamp, "/camera"); } } // end namespace loam diff --git a/src/lib/TransformMaintenance.cpp b/src/lib/TransformMaintenance.cpp index 277ad5f..12c004d 100644 --- a/src/lib/TransformMaintenance.cpp +++ b/src/lib/TransformMaintenance.cpp @@ -30,11 +30,17 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "TransformMaintenance.h" +#include "loam_velodyne/TransformMaintenance.h" namespace loam { +using std::sin; +using std::cos; +using std::asin; +using std::atan2; + + TransformMaintenance::TransformMaintenance() { // initialize odometry and odometry tf messages diff --git a/src/lib/VelodyneScanRegistration.cpp b/src/lib/VelodyneScanRegistration.cpp index 467cd6a..84c457d 100644 --- a/src/lib/VelodyneScanRegistration.cpp +++ b/src/lib/VelodyneScanRegistration.cpp @@ -30,7 +30,10 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "VelodyneScanRegistration.h" +#include "loam_velodyne/VelodyneScanRegistration.h" +#include "math_utils.h" + +#include namespace loam { @@ -120,7 +123,7 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las } // calculate vertical point angle and scan ID - float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z)) * 180 / float(M_PI); + float angle = rad2deg(std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z))); int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : 0.5)); int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScans - 1); if (scanID > (_nScans - 1) || scanID < 0 ){ diff --git a/src/lib/math_utils.h b/src/lib/math_utils.h index 28e26d8..9e3651b 100644 --- a/src/lib/math_utils.h +++ b/src/lib/math_utils.h @@ -1,142 +1,13 @@ #ifndef LOAM_MATH_UTILS_H #define LOAM_MATH_UTILS_H -#include -#include - - -namespace loam { - -/** \brief Vector4f decorator for convenient handling. - * - */ -class Vector3 : public Eigen::Vector4f -{ -public: - Vector3(float x, float y, float z) - : Eigen::Vector4f(x, y, z, 0) {} - - Vector3(void) - : Eigen::Vector4f(0, 0, 0, 0) {} - - template - Vector3(const Eigen::MatrixBase& other) - : Eigen::Vector4f(other) {} - - Vector3(const pcl::PointXYZI& p) - : Eigen::Vector4f(p.x, p.y, p.z, 0) {} - - template - Vector3& operator=(const Eigen::MatrixBase & rhs) { - this->Eigen::Vector4f::operator=(rhs); - return *this; - } - - Vector3& operator=(const pcl::PointXYZ& rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - Vector3& operator=(const pcl::PointXYZI& rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - float x() const { return (*this)(0); } - float y() const { return (*this)(1); } - float z() const { return (*this)(2); } - - float& x() { return (*this)(0); } - float& y() { return (*this)(1); } - float& z() { return (*this)(2); } - - // easy conversion - operator pcl::PointXYZI() - { - pcl::PointXYZI dst; - dst.x = x(); - dst.y = y(); - dst.z = z(); - dst.intensity = 0; - return dst; - } -}; - - - -/** \brief Class for holding an angle. - * - * This class provides buffered access to sine and cosine values to the represented angular value. - */ -class Angle{ -public: - Angle() - : _radian(0.0), - _cos(1.0), - _sin(0.0) {} - - Angle(float radValue) - : _radian(radValue), - _cos(std::cos(radValue)), - _sin(std::sin(radValue)) {} - - Angle(const Angle& other) - : _radian(other._radian), - _cos(other._cos), - _sin(other._sin) {} - - void operator =(const Angle& rhs){ - _radian = (rhs._radian); - _cos = (rhs._cos); - _sin = (rhs._sin); - } - void operator +=( const float& radValue) { *this = ( _radian + radValue) ; } +#include "loam_velodyne/loam_types.h" - void operator +=( const Angle& other) { *this = ( _radian + other._radian ); } - - void operator -=( const float& radValue) { *this = ( _radian - radValue ); } - - void operator -=( const Angle& other) { *this = ( _radian - other._radian ); } - - Angle operator-() const - { - Angle out; - out._radian = -_radian; - out._cos = _cos; - out._sin = -(_sin); - return out; - } - - float rad() const { return _radian; } - - float deg() const { return _radian * 180 / M_PI; } - - float cos() const { return _cos; } - - float sin() const { return _sin; } - -private: - float _radian; ///< angle value in radian - float _cos; ///< cosine of the angle - float _sin; ///< sine of the angle -}; - - - -/** \brief Twist composed by three angles and a three-dimensional position. */ -struct Twist { - Angle rot_x; - Angle rot_y; - Angle rot_z; - Vector3 pos; -}; +#include +namespace loam { /** \brief Convert the given radian angle to degrees. * @@ -193,7 +64,8 @@ inline float deg2rad(float degrees) * @param b The second point. * @return The squared difference between point a and b. */ -inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b) +template +inline float calcSquaredDiff(const PointT& a, const PointT& b) { float diffX = a.x - b.x; float diffY = a.y - b.y; @@ -211,7 +83,8 @@ inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b) * @param wb The weighting factor for the SECOND point. * @return The squared difference between point a and b. */ -inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b, const float& wb) +template +inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb) { float diffX = a.x - b.x * wb; float diffY = a.y - b.y * wb; @@ -226,7 +99,8 @@ inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b, c * @param p The point. * @return The distance to the point. */ -inline float calcPointDistance(const pcl::PointXYZI& p) +template +inline float calcPointDistance(const PointT& p) { return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z); } @@ -238,7 +112,8 @@ inline float calcPointDistance(const pcl::PointXYZI& p) * @param p The point. * @return The squared distance to the point. */ -inline float calcSquaredPointDistance(const pcl::PointXYZI& p) +template +inline float calcSquaredPointDistance(const PointT& p) { return p.x * p.x + p.y * p.y + p.z * p.z; } @@ -262,7 +137,8 @@ inline void rotX(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotX(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotX(PointT& p, const Angle& ang) { float y = p.y; p.y = ang.cos() * y - ang.sin() * p.z; @@ -288,7 +164,8 @@ inline void rotY(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotY(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotY(PointT& p, const Angle& ang) { float x = p.x; p.x = ang.cos() * x + ang.sin() * p.z; @@ -314,7 +191,8 @@ inline void rotZ(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotZ(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotZ(PointT& p, const Angle& ang) { float x = p.x; p.x = ang.cos() * x - ang.sin() * p.y; @@ -347,7 +225,8 @@ inline void rotateZXY(Vector3& v, * @param angX the rotation angle around the x-axis * @param angY the rotation angle around the y-axis */ -inline void rotateZXY(pcl::PointXYZI& p, +template +inline void rotateZXY(PointT& p, const Angle& angZ, const Angle& angX, const Angle& angY) @@ -383,7 +262,8 @@ inline void rotateYXZ(Vector3& v, * @param angX the rotation angle around the x-axis * @param angZ the rotation angle around the z-axis */ -inline void rotateYXZ(pcl::PointXYZI& p, +template +inline void rotateYXZ(PointT& p, const Angle& angY, const Angle& angX, const Angle& angZ) diff --git a/src/transform_maintenance_node.cpp b/src/transform_maintenance_node.cpp index 87ab611..2142d71 100644 --- a/src/transform_maintenance_node.cpp +++ b/src/transform_maintenance_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/TransformMaintenance.h" +#include "loam_velodyne/TransformMaintenance.h" /** Main node entry point. */ diff --git a/src/velodyne_scan_registration_node.cpp b/src/velodyne_scan_registration_node.cpp index 3615ae5..678804c 100644 --- a/src/velodyne_scan_registration_node.cpp +++ b/src/velodyne_scan_registration_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/VelodyneScanRegistration.h" +#include "loam_velodyne/VelodyneScanRegistration.h" /** Main node entry point. */ From 55d90c49578cd1af91ed288ab86cb9153c591014 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sun, 3 Dec 2017 16:44:53 +0100 Subject: [PATCH 7/9] Use ros::Time instances instead of double when available. --- include/loam_velodyne/LaserMapping.h | 8 +- include/loam_velodyne/LaserOdometry.h | 23 ++-- src/lib/LaserMapping.cpp | 30 +++--- src/lib/LaserOdometry.cpp | 145 +++++++++++++------------- 4 files changed, 101 insertions(+), 105 deletions(-) diff --git a/include/loam_velodyne/LaserMapping.h b/include/loam_velodyne/LaserMapping.h index d1e8af5..d86ed2f 100644 --- a/include/loam_velodyne/LaserMapping.h +++ b/include/loam_velodyne/LaserMapping.h @@ -190,10 +190,10 @@ class LaserMapping { std::vector _laserCloudValidInd; std::vector _laserCloudSurroundInd; - double _timeLaserCloudCornerLast; ///< time of current last corner cloud - double _timeLaserCloudSurfLast; ///< time of current last surface cloud - double _timeLaserCloudFullRes; ///< time of current full resolution cloud - double _timeLaserOdometry; ///< time of current laser odometry + ros::Time _timeLaserCloudCornerLast; ///< time of current last corner cloud + ros::Time _timeLaserCloudSurfLast; ///< time of current last surface cloud + ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud + ros::Time _timeLaserOdometry; ///< time of current laser odometry bool _newLaserCloudCornerLast; ///< flag if a new last corner cloud has been received bool _newLaserCloudSurfLast; ///< flag if a new last surface cloud has been received diff --git a/include/loam_velodyne/LaserOdometry.h b/include/loam_velodyne/LaserOdometry.h index e77fa15..aa44d6a 100644 --- a/include/loam_velodyne/LaserOdometry.h +++ b/include/loam_velodyne/LaserOdometry.h @@ -151,22 +151,22 @@ class LaserOdometry { pcl::PointCloud::Ptr _surfPointsFlat; ///< flat surface points cloud pcl::PointCloud::Ptr _surfPointsLessFlat; ///< less flat surface points cloud pcl::PointCloud::Ptr _laserCloud; ///< full resolution cloud - pcl::PointCloud::Ptr _imuTrans; ///< IMU transformation information pcl::PointCloud::Ptr _lastCornerCloud; ///< last corner points cloud pcl::PointCloud::Ptr _lastSurfaceCloud; ///< last surface points cloud + pcl::PointCloud::Ptr _laserCloudOri; ///< point selection pcl::PointCloud::Ptr _coeffSel; ///< point selection coefficients nanoflann::KdTreeFLANN _lastCornerKDTree; ///< last corner cloud KD-tree nanoflann::KdTreeFLANN _lastSurfaceKDTree; ///< last surface cloud KD-tree - double _timeCornerPointsSharp; ///< time of current sharp corner cloud - double _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud - double _timeSurfPointsFlat; ///< time of current flat surface cloud - double _timeSurfPointsLessFlat; ///< time of current less flat surface cloud - double _timeLaserCloudFullRes; ///< time of current full resolution cloud - double _timeImuTrans; ///< time of current IMU transformation information + ros::Time _timeCornerPointsSharp; ///< time of current sharp corner cloud + ros::Time _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud + ros::Time _timeSurfPointsFlat; ///< time of current flat surface cloud + ros::Time _timeSurfPointsLessFlat; ///< time of current less flat surface cloud + ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud + ros::Time _timeImuTrans; ///< time of current IMU transformation information bool _newCornerPointsSharp; ///< flag if a new sharp corner cloud has been received bool _newCornerPointsLessSharp; ///< flag if a new less sharp corner cloud has been received @@ -175,12 +175,6 @@ class LaserOdometry { bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received bool _newImuTrans; ///< flag if a new IMU transformation information cloud has been received - nav_msgs::Odometry _laserOdometry; ///< laser odometry message - tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation - - size_t _lastCornerCloudSize; ///< size of the last corner cloud - size_t _lastSurfaceCloudSize; ///< size of the last surface cloud - std::vector _pointSearchCornerInd1; ///< first corner point search index buffer std::vector _pointSearchCornerInd2; ///< second corner point search index buffer @@ -197,6 +191,9 @@ class LaserOdometry { Vector3 _imuShiftFromStart; Vector3 _imuVeloFromStart; + nav_msgs::Odometry _laserOdometryMsg; ///< laser odometry message + tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation + ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher diff --git a/src/lib/LaserMapping.cpp b/src/lib/LaserMapping.cpp index 940f528..88221e1 100644 --- a/src/lib/LaserMapping.cpp +++ b/src/lib/LaserMapping.cpp @@ -207,17 +207,17 @@ void LaserMapping::transformUpdate() if (_imuHistory.size() > 0) { size_t imuIdx = 0; - while (imuIdx < _imuHistory.size() - 1 && _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) { + while (imuIdx < _imuHistory.size() - 1 && (_timeLaserOdometry - _imuHistory[imuIdx].stamp).toSec() + _scanPeriod > 0) { imuIdx++; } IMUState2 imuCur; - if (imuIdx == 0 || _timeLaserOdometry + _scanPeriod > _imuHistory[imuIdx].stamp.toSec()) { + if (imuIdx == 0 || (_timeLaserOdometry - _imuHistory[imuIdx].stamp).toSec() + _scanPeriod > 0) { // scan time newer then newest or older than oldest IMU message imuCur = _imuHistory[imuIdx]; } else { - float ratio = (_imuHistory[imuIdx].stamp.toSec() - _timeLaserOdometry - _scanPeriod) + float ratio = ((_imuHistory[imuIdx].stamp - _timeLaserOdometry).toSec() - _scanPeriod) / (_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp).toSec(); IMUState2::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, imuCur); @@ -263,7 +263,7 @@ void LaserMapping::pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::Point void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg) { - _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp.toSec(); + _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp; _laserCloudCornerLast->clear(); pcl::fromROSMsg(*cornerPointsLastMsg, *_laserCloudCornerLast); @@ -275,7 +275,7 @@ void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2Con void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg) { - _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp.toSec(); + _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp; _laserCloudSurfLast->clear(); pcl::fromROSMsg(*surfacePointsLastMsg, *_laserCloudSurfLast); @@ -287,7 +287,7 @@ void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2Const void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) { - _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec(); + _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp; _laserCloudFullRes->clear(); pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloudFullRes); @@ -299,7 +299,7 @@ void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstP void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) { - _timeLaserOdometry = laserOdometry->header.stamp.toSec(); + _timeLaserOdometry = laserOdometry->header.stamp; double roll, pitch, yaw; geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; @@ -367,9 +367,9 @@ bool LaserMapping::hasNewData() { return _newLaserCloudCornerLast && _newLaserCloudSurfLast && _newLaserCloudFullRes && _newLaserOdometry && - fabs(_timeLaserCloudCornerLast - _timeLaserOdometry) < 0.005 && - fabs(_timeLaserCloudSurfLast - _timeLaserOdometry) < 0.005 && - fabs(_timeLaserCloudFullRes - _timeLaserOdometry) < 0.005; + fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) < 0.005 && + fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) < 0.005 && + fabs((_timeLaserCloudFullRes - _timeLaserOdometry).toSec()) < 0.005; } @@ -1037,8 +1037,6 @@ void LaserMapping::optimizeTransformTobeMapped() void LaserMapping::publishResult() { - ros::Time odometryTime = ros::Time().fromSec(_timeLaserOdometry); - // publish new map cloud according to the input output ratio _mapFrameCount++; if (_mapFrameCount >= _mapFrameNum) { @@ -1059,7 +1057,7 @@ void LaserMapping::publishResult() _downSizeFilterCorner.filter(*_laserCloudSurroundDS); // publish new map cloud - publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, odometryTime, "/camera_init"); + publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, _timeLaserOdometry, "/camera_init"); } @@ -1070,7 +1068,7 @@ void LaserMapping::publishResult() } // publish transformed full resolution input cloud - publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, odometryTime, "/camera_init"); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, _timeLaserOdometry, "/camera_init"); // publish odometry after mapped transformations @@ -1079,7 +1077,7 @@ void LaserMapping::publishResult() -_transformAftMapped.rot_x.rad(), -_transformAftMapped.rot_y.rad()); - _odomAftMapped.header.stamp = odometryTime; + _odomAftMapped.header.stamp = _timeLaserOdometry; _odomAftMapped.pose.pose.orientation.x = -geoQuat.y; _odomAftMapped.pose.pose.orientation.y = -geoQuat.z; _odomAftMapped.pose.pose.orientation.z = geoQuat.x; @@ -1095,7 +1093,7 @@ void LaserMapping::publishResult() _odomAftMapped.twist.twist.linear.z = _transformBefMapped.pos.z(); _pubOdomAftMapped.publish(_odomAftMapped); - _aftMappedTrans.stamp_ = odometryTime; + _aftMappedTrans.stamp_ = _timeLaserOdometry; _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); _aftMappedTrans.setOrigin(tf::Vector3(_transformAftMapped.pos.x(), _transformAftMapped.pos.y(), diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp index 172882a..32eab9d 100644 --- a/src/lib/LaserOdometry.cpp +++ b/src/lib/LaserOdometry.cpp @@ -61,15 +61,14 @@ LaserOdometry::LaserOdometry(const float& scanPeriod, _surfPointsFlat(new pcl::PointCloud()), _surfPointsLessFlat(new pcl::PointCloud()), _laserCloud(new pcl::PointCloud()), - _imuTrans(new pcl::PointCloud()), _lastCornerCloud(new pcl::PointCloud()), _lastSurfaceCloud(new pcl::PointCloud()), _laserCloudOri(new pcl::PointCloud()), _coeffSel(new pcl::PointCloud()) { // initialize odometry and odometry tf messages - _laserOdometry.header.frame_id = "/camera_init"; - _laserOdometry.child_frame_id = "/laser_odom"; + _laserOdometryMsg.header.frame_id = "/camera_init"; + _laserOdometryMsg.child_frame_id = "/laser_odom"; _laserOdometryTrans.frame_id_ = "/camera_init"; _laserOdometryTrans.child_frame_id_ = "/laser_odom"; @@ -119,10 +118,10 @@ void LaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& p po.z = pi.z - s * _transform.pos.z(); po.intensity = pi.intensity; - Angle rx = s * _transform.rot_x.rad(); - Angle ry = s * _transform.rot_y.rad(); - Angle rz = s * _transform.rot_z.rad(); - rotateZXY(po, -rz, -rx, -ry); + Angle rx = -s * _transform.rot_x.rad(); + Angle ry = -s * _transform.rot_y.rad(); + Angle rz = -s * _transform.rot_z.rad(); + rotateZXY(po, rz, rx, ry); } @@ -141,10 +140,10 @@ size_t LaserOdometry::transformToEnd(pcl::PointCloud::Ptr& cloud point.z -= s * _transform.pos.z(); point.intensity = int(point.intensity); - Angle rx = s * _transform.rot_x.rad(); - Angle ry = s * _transform.rot_y.rad(); - Angle rz = s * _transform.rot_z.rad(); - rotateZXY(point, -rz, -rx, -ry); + Angle rx = -s * _transform.rot_x.rad(); + Angle ry = -s * _transform.rot_y.rad(); + Angle rz = -s * _transform.rot_z.rad(); + rotateZXY(point, rz, rx, ry); rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z); point.x += _transform.pos.x() - _imuShiftFromStart.x(); @@ -254,7 +253,7 @@ void LaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz, void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg) { - _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp.toSec(); + _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp; _cornerPointsSharp->clear(); pcl::fromROSMsg(*cornerPointsSharpMsg, *_cornerPointsSharp); @@ -267,7 +266,7 @@ void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPt void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg) { - _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp.toSec(); + _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp; _cornerPointsLessSharp->clear(); pcl::fromROSMsg(*cornerPointsLessSharpMsg, *_cornerPointsLessSharp); @@ -280,7 +279,7 @@ void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2Con void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg) { - _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp.toSec(); + _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp; _surfPointsFlat->clear(); pcl::fromROSMsg(*surfPointsFlatMsg, *_surfPointsFlat); @@ -293,7 +292,7 @@ void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg) { - _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp.toSec(); + _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp; _surfPointsLessFlat->clear(); pcl::fromROSMsg(*surfPointsLessFlatMsg, *_surfPointsLessFlat); @@ -306,7 +305,7 @@ void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2Cons void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) { - _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec(); + _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp; _laserCloud->clear(); pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloud); @@ -319,21 +318,21 @@ void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2Const void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg) { - _timeImuTrans = imuTransMsg->header.stamp.toSec(); + _timeImuTrans = imuTransMsg->header.stamp; - _imuTrans->clear(); - pcl::fromROSMsg(*imuTransMsg, *_imuTrans); + pcl::PointCloud imuTrans; + pcl::fromROSMsg(*imuTransMsg, imuTrans); - _imuPitchStart = _imuTrans->points[0].x; - _imuYawStart = _imuTrans->points[0].y; - _imuRollStart = _imuTrans->points[0].z; + _imuPitchStart = imuTrans.points[0].x; + _imuYawStart = imuTrans.points[0].y; + _imuRollStart = imuTrans.points[0].z; - _imuPitchEnd = _imuTrans->points[1].x; - _imuYawEnd = _imuTrans->points[1].y; - _imuRollEnd = _imuTrans->points[1].z; + _imuPitchEnd = imuTrans.points[1].x; + _imuYawEnd = imuTrans.points[1].y; + _imuRollEnd = imuTrans.points[1].z; - _imuShiftFromStart = _imuTrans->points[2]; - _imuVeloFromStart = _imuTrans->points[3]; + _imuShiftFromStart = imuTrans.points[2]; + _imuVeloFromStart = imuTrans.points[3]; _newImuTrans = true; } @@ -375,11 +374,11 @@ bool LaserOdometry::hasNewData() { return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat && _newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans && - fabs(_timeCornerPointsSharp - _timeSurfPointsLessFlat) < 0.005 && - fabs(_timeCornerPointsLessSharp - _timeSurfPointsLessFlat) < 0.005 && - fabs(_timeSurfPointsFlat - _timeSurfPointsLessFlat) < 0.005 && - fabs(_timeLaserCloudFullRes - _timeSurfPointsLessFlat) < 0.005 && - fabs(_timeImuTrans - _timeSurfPointsLessFlat) < 0.005; + fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 && + fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 && + fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 && + fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 && + fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005; } @@ -398,9 +397,6 @@ void LaserOdometry::process() _cornerPointsLessSharp.swap(_lastCornerCloud); _surfPointsLessFlat.swap(_lastSurfaceCloud); - _lastCornerCloudSize = _lastCornerCloud->points.size(); - _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); - _lastCornerKDTree.setInputCloud(_lastCornerCloud); _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); @@ -418,7 +414,11 @@ void LaserOdometry::process() _frameCount++; _transform.pos -= _imuVeloFromStart * _scanPeriod; - if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + + size_t lastCornerCloudSize = _lastCornerCloud->points.size(); + size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + + if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) { std::vector pointSearchInd(1); std::vector pointSearchSqDis(1); std::vector indices; @@ -756,13 +756,13 @@ void LaserOdometry::process() _transform.pos.y() += matX(4, 0); _transform.pos.z() += matX(5, 0); - if( isnan(_transform.rot_x.rad()) ) _transform.rot_x = Angle(); - if( isnan(_transform.rot_y.rad()) ) _transform.rot_y = Angle(); - if( isnan(_transform.rot_z.rad()) ) _transform.rot_z = Angle(); + if( !pcl_isfinite(_transform.rot_x.rad()) ) _transform.rot_x = Angle(); + if( !pcl_isfinite(_transform.rot_y.rad()) ) _transform.rot_y = Angle(); + if( !pcl_isfinite(_transform.rot_z.rad()) ) _transform.rot_z = Angle(); - if( isnan(_transform.pos.x()) ) _transform.pos.x() = 0.0; - if( isnan(_transform.pos.y()) ) _transform.pos.y() = 0.0; - if( isnan(_transform.pos.z()) ) _transform.pos.z() = 0.0; + if( !pcl_isfinite(_transform.pos.x()) ) _transform.pos.x() = 0.0; + if( !pcl_isfinite(_transform.pos.y()) ) _transform.pos.y() = 0.0; + if( !pcl_isfinite(_transform.pos.z()) ) _transform.pos.z() = 0.0; float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) + pow(rad2deg(matX(1, 0)), 2) + @@ -778,7 +778,6 @@ void LaserOdometry::process() } Angle rx, ry, rz; - accumulateRotation(_transformSum.rot_x, _transformSum.rot_y, _transformSum.rot_z, @@ -803,33 +802,16 @@ void LaserOdometry::process() _transformSum.rot_z = rz; _transformSum.pos = trans; - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); - - _laserOdometry.header.stamp = ros::Time().fromSec(_timeSurfPointsLessFlat); - _laserOdometry.pose.pose.orientation.x = -geoQuat.y; - _laserOdometry.pose.pose.orientation.y = -geoQuat.z; - _laserOdometry.pose.pose.orientation.z = geoQuat.x; - _laserOdometry.pose.pose.orientation.w = geoQuat.w; - _laserOdometry.pose.pose.position.x = trans.x(); - _laserOdometry.pose.pose.position.y = trans.y(); - _laserOdometry.pose.pose.position.z = trans.z(); - _pubLaserOdometry.publish(_laserOdometry); - - _laserOdometryTrans.stamp_ = ros::Time().fromSec(_timeSurfPointsLessFlat); - _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - _laserOdometryTrans.setOrigin(tf::Vector3( trans.x(), trans.y(), trans.z()) ); - _tfBroadcaster.sendTransform(_laserOdometryTrans); - transformToEnd(_cornerPointsLessSharp); transformToEnd(_surfPointsLessFlat); _cornerPointsLessSharp.swap(_lastCornerCloud); _surfPointsLessFlat.swap(_lastSurfaceCloud); - _lastCornerCloudSize = _lastCornerCloud->points.size(); - _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + lastCornerCloudSize = _lastCornerCloud->points.size(); + lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); - if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) { _lastCornerKDTree.setInputCloud(_lastCornerCloud); _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); } @@ -841,17 +823,36 @@ void LaserOdometry::process() void LaserOdometry::publishResult() { - // publish results according to the input output ratio - if (_ioRatio > 1 && _frameCount % _ioRatio != 1) { - return; - } + // publish odometry tranformations + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(_transformSum.rot_z.rad(), + -_transformSum.rot_x.rad(), + -_transformSum.rot_y.rad()); + + _laserOdometryMsg.header.stamp = _timeSurfPointsLessFlat; + _laserOdometryMsg.pose.pose.orientation.x = -geoQuat.y; + _laserOdometryMsg.pose.pose.orientation.y = -geoQuat.z; + _laserOdometryMsg.pose.pose.orientation.z = geoQuat.x; + _laserOdometryMsg.pose.pose.orientation.w = geoQuat.w; + _laserOdometryMsg.pose.pose.position.x = _transformSum.pos.x(); + _laserOdometryMsg.pose.pose.position.y = _transformSum.pos.y(); + _laserOdometryMsg.pose.pose.position.z = _transformSum.pos.z(); + _pubLaserOdometry.publish(_laserOdometryMsg); + + _laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat; + _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + _laserOdometryTrans.setOrigin(tf::Vector3( _transformSum.pos.x(), _transformSum.pos.y(), _transformSum.pos.z()) ); + _tfBroadcaster.sendTransform(_laserOdometryTrans); + - ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); - transformToEnd(_laserCloud); // transform full resolution cloud to sweep end before sending it + // publish cloud results according to the input output ratio + if (_ioRatio < 2 || _frameCount % _ioRatio == 1) { + ros::Time sweepTime = _timeSurfPointsLessFlat; + transformToEnd(_laserCloud); // transform full resolution cloud to sweep end before sending it - publishCloudMsg(_pubLaserCloudCornerLast, *_lastCornerCloud, sweepTime, "/camera"); - publishCloudMsg(_pubLaserCloudSurfLast, *_lastSurfaceCloud, sweepTime, "/camera"); - publishCloudMsg(_pubLaserCloudFullRes, *_laserCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudCornerLast, *_lastCornerCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudSurfLast, *_lastSurfaceCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloud, sweepTime, "/camera"); + } } } // end namespace loam From 44fb3e41ca5b2db559fc0dad717f65ccab2f5351 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sun, 3 Dec 2017 22:00:55 +0100 Subject: [PATCH 8/9] Further code cleanup in scan registration components. * Replaced scan start and end indices vectors with one std::pair vector. * Removed _nScans variable from ScanRegistration class and replaced its usage with the size of the scan indices vector. This way feature extration will automatically run for all available scans. * Extracted neighbor marking on feature selection into own function. * Removed some duplicate code for IMU interpolation. * Split interpolation of IMU state and point transformation into two functions and store all relevant information needed for the transformation. This way, multi-laser lidars can use the same IMU interpolation for registering all laser measurements to a given point in time. * Removed point cloud getters. * Automatically generate IMU transformation information cloud when publishing the result. * Removed unnecessary shared pointer declaration of internal cloud buffers. * Some minor simplifications. --- include/loam_velodyne/ScanRegistration.h | 111 ++++--- .../loam_velodyne/VelodyneScanRegistration.h | 14 +- src/lib/ScanRegistration.cpp | 282 +++++++++--------- src/lib/VelodyneScanRegistration.cpp | 68 ++--- 4 files changed, 221 insertions(+), 254 deletions(-) diff --git a/include/loam_velodyne/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h index cb194f9..e86f8e6 100644 --- a/include/loam_velodyne/ScanRegistration.h +++ b/include/loam_velodyne/ScanRegistration.h @@ -34,6 +34,7 @@ #define LOAM_SCANREGISTRATION_H +#include "common.h" #include "loam_types.h" #include "CircularBuffer.h" @@ -46,6 +47,11 @@ namespace loam { +/** \brief A pair describing the start end end index of a range. */ +typedef std::pair IndexRange; + + + /** Point label options. */ enum PointLabel { CORNER_SHARP = 2, ///< sharp corner point @@ -252,9 +258,8 @@ typedef struct IMUState { class ScanRegistration { public: explicit ScanRegistration(const float& scanPeriod, - const uint16_t& nScans = 0, - const size_t& imuHistorySize = 200, - const RegistrationParams& config = RegistrationParams()); + const RegistrationParams& config = RegistrationParams(), + const size_t& imuHistorySize = 200); /** \brief Setup component. * @@ -270,57 +275,30 @@ class ScanRegistration { */ virtual void handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn); - /** \brief Retrieve the current full resolution input cloud. - * - * @return the current full resolution input cloud - */ - pcl::PointCloud::Ptr getFullResCloud() { return _laserCloud; }; - - /** \brief Retrieve the current sharp corner input sub cloud. - * - * @return the current sharp corner input sub cloud - */ - pcl::PointCloud::Ptr getCornerPointsSharp() { return _cornerPointsSharp; }; - - /** \brief Retrieve the current less sharp corner input sub cloud. - * - * @return the current less sharp corner input sub cloud - */ - pcl::PointCloud::Ptr getCornerPointsLessSharp() { return _cornerPointsLessSharp; }; - - /** \brief Retrieve the current flat surface input sub cloud. - * - * @return the current flat surface input sub cloud - */ - pcl::PointCloud::Ptr getSurfacePointsFlat() { return _surfacePointsFlat; }; - - /** \brief Retrieve the current less flat surface input sub cloud. - * - * @return the current less flat surface input sub cloud - */ - pcl::PointCloud::Ptr getSurfacePointsLessFlat() { return _surfacePointsLessFlat; }; - /** \brief Retrieve the current IMU transformation information. +protected: + /** \brief Prepare for next scan / sweep. * - * @return the current IMU transformation information + * @param scanTime the current scan time + * @param newSweep indicator if a new sweep has started */ - pcl::PointCloud::Ptr getIMUTrans() { return _imuTrans; }; + void reset(const ros::Time& scanTime, + const bool& newSweep = true); + /** \breif Check is IMU data is available. */ + inline bool hasIMUData() { return _imuHistory.size() > 0; }; -protected: - /** \brief Prepare for next sweep: Reset internal cloud buffers and re-initialize start IMU state. + /** \brief Set up the current IMU transformation for the specified relative time. * - * @param scanTime the current scan time + * @param relTime the time relative to the scan time */ - void reset(const ros::Time& scanTime); + void setIMUTransformFor(const float& relTime); - /** \brief Project the given point to the start of the sweep, using the current IMU state and relative time. + /** \brief Project the given point to the start of the sweep, using the current IMU state and position shift. * * @param point the point to project - * @param relTime the relative point measurement time */ - void transformToStartIMU(pcl::PointXYZI& point, - const float& relTime); + void transformToStartIMU(pcl::PointXYZI& point); /** \brief Extract features from current laser cloud. * @@ -344,36 +322,51 @@ class ScanRegistration { void setScanBuffersFor(const size_t& startIdx, const size_t& endIdx); - /** \brief Set sweep end IMU transformation information. + /** \brief Mark a point and its neighbors as picked. + * + * This method will mark neighboring points within the curvature region as picked, + * as long as they remain within close distance to each other. * - * @param sweepDuration the total duration of the current sweep + * @param cloudIdx the index of the picked point in the full resolution cloud + * @param scanIdx the index of the picked point relative to the current scan */ - void setIMUTrans(const double& sweepDuration); + void markAsPicked(const size_t& cloudIdx, + const size_t& scanIdx); /** \brief Publish the current result via the respective topics. */ void publishResult(); +private: + /** \brief Try to interpolate the IMU state for the given time. + * + * @param relTime the time relative to the scan time + * @param outputState the output state instance + */ + void interpolateIMUStateFor(const float& relTime, + IMUState& outputState); + + protected: - const uint16_t _nScans; ///< number of scans per sweep - const float _scanPeriod; ///< time per scan - ros::Time _sweepStamp; ///< time stamp of the beginning of current sweep - RegistrationParams _config; ///< registration parameter + const float _scanPeriod; ///< time per scan + RegistrationParams _config; ///< registration parameter + ros::Time _sweepStart; ///< time stamp of beginning of current sweep + ros::Time _scanTime; ///< time stamp of most recent scan IMUState _imuStart; ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan IMUState _imuCur; ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point - size_t _imuStartIdx; ///< the index in the IMU history of the first IMU state received after the current scan time + Vector3 _imuPositionShift; ///< position shift between accumulated IMU position and interpolated IMU position + size_t _imuIdx; ///< the current index in the IMU history CircularBuffer _imuHistory; ///< history of IMU states for cloud registration - pcl::PointCloud::Ptr _laserCloud; ///< full resolution input cloud - std::vector _scanStartIndices; ///< start indices of the individual scans - std::vector _scanEndIndices; ///< end indices of the individual scans + pcl::PointCloud _laserCloud; ///< full resolution input cloud + std::vector _scanIndices; ///< start and end indices of the individual scans withing the full resolution cloud - pcl::PointCloud::Ptr _cornerPointsSharp; ///< sharp corner points cloud - pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud - pcl::PointCloud::Ptr _surfacePointsFlat; ///< flat surface points cloud - pcl::PointCloud::Ptr _surfacePointsLessFlat; ///< less flat surface points cloud - pcl::PointCloud::Ptr _imuTrans; ///< IMU transformation information + pcl::PointCloud _cornerPointsSharp; ///< sharp corner points cloud + pcl::PointCloud _cornerPointsLessSharp; ///< less sharp corner points cloud + pcl::PointCloud _surfacePointsFlat; ///< flat surface points cloud + pcl::PointCloud _surfacePointsLessFlat; ///< less flat surface points cloud + pcl::PointCloud _imuTrans; ///< IMU transformation information std::vector _regionCurvature; ///< point curvature buffer std::vector _regionLabel; ///< point label buffer diff --git a/include/loam_velodyne/VelodyneScanRegistration.h b/include/loam_velodyne/VelodyneScanRegistration.h index cbe474e..f09eab6 100644 --- a/include/loam_velodyne/VelodyneScanRegistration.h +++ b/include/loam_velodyne/VelodyneScanRegistration.h @@ -47,9 +47,9 @@ namespace loam { class VelodyneScanRegistration : virtual public ScanRegistration { public: VelodyneScanRegistration(const float& scanPeriod, - const uint16_t& nScans, - const size_t& imuHistorySize = 200, - const RegistrationParams& config = RegistrationParams()); + const uint16_t& nScanRings, + const RegistrationParams& config = RegistrationParams(), + const size_t& imuHistorySize = 200); /** \brief Setup component in active mode. @@ -60,11 +60,11 @@ class VelodyneScanRegistration : virtual public ScanRegistration { bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode); - /** \brief Process a new input cloud message. + /** \brief Handler method for input cloud messages. * * @param laserCloudMsg the new input cloud message to process */ - void processCloudMessage(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); + void handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg); /** \brief Process a new input cloud. * @@ -75,7 +75,9 @@ class VelodyneScanRegistration : virtual public ScanRegistration { const ros::Time& scanTime); protected: - int _systemDelay; ///< system startup delay counter + int _systemDelay; ///< system startup delay counter + const uint16_t _nScanRings; ///< number of scan rings + ros::Subscriber _subLaserCloud; ///< input cloud message subscriber }; diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index 166742e..0b23ce0 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -31,7 +31,6 @@ // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. #include "loam_velodyne/ScanRegistration.h" -#include "loam_velodyne/common.h" #include "math_utils.h" #include @@ -41,30 +40,28 @@ namespace loam { ScanRegistration::ScanRegistration(const float& scanPeriod, - const uint16_t& nScans, - const size_t& imuHistorySize, - const RegistrationParams& config) - : _nScans(nScans), - _scanPeriod(scanPeriod), - _sweepStamp(), + const RegistrationParams& config, + const size_t& imuHistorySize) + : _scanPeriod(scanPeriod), _config(config), + _sweepStart(), + _scanTime(), _imuStart(), _imuCur(), - _imuStartIdx(0), + _imuIdx(0), _imuHistory(imuHistorySize), - _laserCloud(new pcl::PointCloud()), - _cornerPointsSharp(new pcl::PointCloud()), - _cornerPointsLessSharp(new pcl::PointCloud()), - _surfacePointsFlat(new pcl::PointCloud()), - _surfacePointsLessFlat(new pcl::PointCloud()), - _imuTrans(new pcl::PointCloud(4, 1)), + _laserCloud(), + _cornerPointsSharp(), + _cornerPointsLessSharp(), + _surfacePointsFlat(), + _surfacePointsLessFlat(), + _imuTrans(4, 1), _regionCurvature(), _regionLabel(), _regionSortIndices(), _scanNeighborPicked() { - _scanStartIndices.assign(nScans, 0); - _scanEndIndices.assign(nScans, 0); + }; @@ -131,56 +128,54 @@ void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn) -void ScanRegistration::reset(const ros::Time& scanTime) +void ScanRegistration::reset(const ros::Time& scanTime, + const bool& newSweep) { - _sweepStamp = scanTime; + _scanTime = scanTime; - // clear cloud buffers - _laserCloud->clear(); - _cornerPointsSharp->clear(); - _cornerPointsLessSharp->clear(); - _surfacePointsFlat->clear(); - _surfacePointsLessFlat->clear(); + // re-initialize IMU start index and state + _imuIdx = 0; + if (hasIMUData()) { + interpolateIMUStateFor(0, _imuStart); + } + // clear internal cloud buffers at the beginning of a sweep + if (newSweep) { + _sweepStart = scanTime; - // reset scan indices vectors - _scanStartIndices.assign(_nScans, 0); - _scanEndIndices.assign(_nScans, 0); + // clear cloud buffers + _laserCloud.clear(); + _cornerPointsSharp.clear(); + _cornerPointsLessSharp.clear(); + _surfacePointsFlat.clear(); + _surfacePointsLessFlat.clear(); + // clear scan indices vector + _scanIndices.clear(); + } +} - // re-initialize IMU start state and index - _imuStartIdx = 0; - if (_imuHistory.size() > 0) { - while (_imuStartIdx < _imuHistory.size() - 1 && (scanTime - _imuHistory[_imuStartIdx].stamp).toSec() > 0) { - _imuStartIdx++; - } - // fetch / interpolate IMU start state - if (_imuStartIdx == 0 || (scanTime - _imuHistory[_imuStartIdx].stamp).toSec() > 0) { - // scan time newer then newest or older than oldest IMU message - _imuStart = _imuHistory[_imuStartIdx]; - } else { - float ratio = (_imuHistory[_imuStartIdx].stamp - scanTime).toSec() - / (_imuHistory[_imuStartIdx].stamp - _imuHistory[_imuStartIdx - 1].stamp).toSec(); - IMUState::interpolate(_imuHistory[_imuStartIdx], _imuHistory[_imuStartIdx - 1], ratio, _imuStart); - } - } +void ScanRegistration::setIMUTransformFor(const float& relTime) +{ + interpolateIMUStateFor(relTime, _imuCur); + + float relSweepTime = (_scanTime - _sweepStart).toSec() + relTime; + _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime; } -void ScanRegistration::transformToStartIMU(pcl::PointXYZI& point, - const float& pointTime) +void ScanRegistration::transformToStartIMU(pcl::PointXYZI& point) { // rotate point to global IMU system rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw); // add global IMU position shift - Vector3 positionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * pointTime; - point.x += positionShift.x(); - point.y += positionShift.y(); - point.z += positionShift.z(); + point.x += _imuPositionShift.x(); + point.y += _imuPositionShift.y(); + point.z += _imuPositionShift.z(); // rotate point back to local IMU system relative to the start IMU state rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); @@ -188,29 +183,21 @@ void ScanRegistration::transformToStartIMU(pcl::PointXYZI& point, -void ScanRegistration::setIMUTrans(const double& sweepDuration) +void ScanRegistration::interpolateIMUStateFor(const float &relTime, + IMUState &outputState) { - _imuTrans->points[0].x = _imuStart.pitch.rad(); - _imuTrans->points[0].y = _imuStart.yaw.rad(); - _imuTrans->points[0].z = _imuStart.roll.rad(); - - _imuTrans->points[1].x = _imuCur.pitch.rad(); - _imuTrans->points[1].y = _imuCur.yaw.rad(); - _imuTrans->points[1].z = _imuCur.roll.rad(); - - Vector3 imuShiftFromStart = _imuCur.position - _imuStart.position - _imuStart.velocity * sweepDuration; - rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); - - _imuTrans->points[2].x = imuShiftFromStart.x(); - _imuTrans->points[2].y = imuShiftFromStart.y(); - _imuTrans->points[2].z = imuShiftFromStart.z(); - - Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity; - rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); + double timeDiff = (_scanTime - _imuHistory[_imuIdx].stamp).toSec() + relTime; + while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) { + _imuIdx++; + timeDiff = (_scanTime - _imuHistory[_imuIdx].stamp).toSec() + relTime; + } - _imuTrans->points[3].x = imuVelocityFromStart.x(); - _imuTrans->points[3].y = imuVelocityFromStart.y(); - _imuTrans->points[3].z = imuVelocityFromStart.z(); + if (_imuIdx == 0 || timeDiff > 0) { + outputState = _imuHistory[_imuIdx]; + } else { + float ratio = -timeDiff / (_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp).toSec(); + IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState); + } } @@ -218,12 +205,11 @@ void ScanRegistration::setIMUTrans(const double& sweepDuration) void ScanRegistration::extractFeatures(const uint16_t& beginIdx) { // extract features from individual scans - for (int i = beginIdx; i < _nScans; i++) { - // ROS_INFO("Extract features for scan %d", i); - + size_t nScans = _scanIndices.size(); + for (size_t i = beginIdx; i < nScans; i++) { pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); - size_t scanStartIdx = _scanStartIndices[i]; - size_t scanEndIdx = _scanEndIndices[i]; + size_t scanStartIdx = _scanIndices[i].first; + size_t scanEndIdx = _scanIndices[i].second; // skip empty scans if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) { @@ -233,7 +219,7 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) // Quick&Dirty fix for relative point time calculation without IMU data /*float scanSize = scanEndIdx - scanStartIdx + 1; for (int j = scanStartIdx; j <= scanEndIdx; j++) { - _laserCloud->points[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize; + _laserCloud[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize; }*/ // reset scan buffers @@ -257,8 +243,6 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) setRegionBuffersFor(sp, ep); - // ROS_INFO("Extract corner features"); - // extract corner features int largestPickedNum = 0; for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) { @@ -272,35 +256,16 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) largestPickedNum++; if (largestPickedNum <= _config.maxCornerSharp) { _regionLabel[regionIdx] = CORNER_SHARP; - _cornerPointsSharp->push_back(_laserCloud->points[idx]); - _cornerPointsLessSharp->push_back(_laserCloud->points[idx]); + _cornerPointsSharp.push_back(_laserCloud[idx]); } else { _regionLabel[regionIdx] = CORNER_LESS_SHARP; - _cornerPointsLessSharp->push_back(_laserCloud->points[idx]); - } - - _scanNeighborPicked[scanIdx] = 1; - for (int l = 1; l <= _config.curvatureRegion; l++) { - float distSquare = calcSquaredDiff(_laserCloud->points[idx + l], _laserCloud->points[idx + l - 1]); - if (distSquare > 0.05) { - break; - } - - _scanNeighborPicked[scanIdx + l] = 1; } - for (int l = 1; l <= _config.curvatureRegion; l++) { - float distSquare = calcSquaredDiff(_laserCloud->points[idx - l], _laserCloud->points[idx - l + 1]); - if (distSquare > 0.05) { - break; - } + _cornerPointsLessSharp.push_back(_laserCloud[idx]); - _scanNeighborPicked[scanIdx - l] = 1; - } + markAsPicked(idx, scanIdx); } } - // ROS_INFO("Extract flat features"); - // extract flat surface features int smallestPickedNum = 0; for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) { @@ -313,34 +278,16 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) smallestPickedNum++; _regionLabel[regionIdx] = SURFACE_FLAT; - _surfacePointsFlat->push_back(_laserCloud->points[idx]); - - _scanNeighborPicked[scanIdx] = 1; - for (int l = 1; l <= _config.curvatureRegion; l++) { - float distSquare = calcSquaredDiff(_laserCloud->points[idx + l], _laserCloud->points[idx + l - 1]); - if (distSquare > 0.05) { - break; - } - - _scanNeighborPicked[scanIdx + l] = 1; - } - for (int l = 1; l <= _config.curvatureRegion; l++) { - float distSquare = calcSquaredDiff(_laserCloud->points[idx - l], _laserCloud->points[idx - l + 1]); - if (distSquare > 0.05) { - break; - } + _surfacePointsFlat.push_back(_laserCloud[idx]); - _scanNeighborPicked[scanIdx - l] = 1; - } + markAsPicked(idx, scanIdx); } } - // ROS_INFO("Extract less flat features"); - // extract less flat surface features for (int k = 0; k < regionSize; k++) { if (_regionLabel[k] <= SURFACE_LESS_FLAT) { - surfPointsLessFlatScan->push_back(_laserCloud->points[sp + k]); + surfPointsLessFlatScan->push_back(_laserCloud[sp + k]); } } } @@ -352,7 +299,7 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize); downSizeFilter.filter(surfPointsLessFlatScanDS); - *_surfacePointsLessFlat += surfPointsLessFlatScanDS; + _surfacePointsLessFlat += surfPointsLessFlatScanDS; } } @@ -361,8 +308,6 @@ void ScanRegistration::extractFeatures(const uint16_t& beginIdx) void ScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx) { - // ROS_INFO("Set region buffers for %d to %d", int(startIdx), int(endIdx)); - // resize buffers size_t regionSize = endIdx - startIdx + 1; _regionCurvature.resize(regionSize); @@ -373,14 +318,14 @@ void ScanRegistration::setRegionBuffersFor(const size_t& startIdx, float pointWeight = -2 * _config.curvatureRegion; for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) { - float diffX = pointWeight * _laserCloud->points[i].x; - float diffY = pointWeight * _laserCloud->points[i].y; - float diffZ = pointWeight * _laserCloud->points[i].z; + float diffX = pointWeight * _laserCloud[i].x; + float diffY = pointWeight * _laserCloud[i].y; + float diffZ = pointWeight * _laserCloud[i].z; for (int j = 1; j <= _config.curvatureRegion; j++) { - diffX += _laserCloud->points[i + j].x + _laserCloud->points[i - j].x; - diffY += _laserCloud->points[i + j].y + _laserCloud->points[i - j].y; - diffZ += _laserCloud->points[i + j].z + _laserCloud->points[i - j].z; + diffX += _laserCloud[i + j].x + _laserCloud[i - j].x; + diffY += _laserCloud[i + j].y + _laserCloud[i - j].y; + diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z; } _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ; @@ -402,17 +347,15 @@ void ScanRegistration::setRegionBuffersFor(const size_t& startIdx, void ScanRegistration::setScanBuffersFor(const size_t& startIdx, const size_t& endIdx) { - // ROS_INFO("Set scan buffers for %d to %d", int(startIdx), int(endIdx)); - // resize buffers size_t scanSize = endIdx - startIdx + 1; _scanNeighborPicked.assign(scanSize, 0); // mark unreliable points as picked for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) { - const pcl::PointXYZI& previousPoint = (_laserCloud->points[i - 1]); - const pcl::PointXYZI& point = (_laserCloud->points[i]); - const pcl::PointXYZI& nextPoint = (_laserCloud->points[i + 1]); + const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]); + const pcl::PointXYZI& point = (_laserCloud[i]); + const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]); float diffNext = calcSquaredDiff(nextPoint, point); @@ -424,10 +367,7 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2; if (weighted_distance < 0.1) { - size_t scanIdx = i - startIdx; - for (int j = 0; j <= _config.curvatureRegion; j++) { - _scanNeighborPicked[scanIdx - j] = 1; - } + std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1); continue; } @@ -435,10 +375,7 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1; if (weighted_distance < 0.1) { - size_t scanIdx = i - startIdx; - for (int j = _config.curvatureRegion + 1; j > 0 ; j--) { - _scanNeighborPicked[scanIdx + j] = 1; - } + std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1); } } } @@ -454,17 +391,64 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, +void ScanRegistration::markAsPicked(const size_t& cloudIdx, + const size_t& scanIdx) +{ + _scanNeighborPicked[scanIdx] = 1; + + for (int i = 1; i <= _config.curvatureRegion; i++) { + if (calcSquaredDiff(_laserCloud[cloudIdx + i], _laserCloud[cloudIdx + i - 1]) > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx + i] = 1; + } + + for (int i = 1; i <= _config.curvatureRegion; i++) { + if (calcSquaredDiff(_laserCloud[cloudIdx - i], _laserCloud[cloudIdx - i + 1]) > 0.05) { + break; + } + + _scanNeighborPicked[scanIdx - i] = 1; + } +} + + + void ScanRegistration::publishResult() { // publish full resolution and feature point clouds - publishCloudMsg(_pubLaserCloud, *_laserCloud, _sweepStamp, "/camera"); - publishCloudMsg(_pubCornerPointsSharp, *_cornerPointsSharp, _sweepStamp, "/camera"); - publishCloudMsg(_pubCornerPointsLessSharp, *_cornerPointsLessSharp, _sweepStamp, "/camera"); - publishCloudMsg(_pubSurfPointsFlat, *_surfacePointsFlat, _sweepStamp, "/camera"); - publishCloudMsg(_pubSurfPointsLessFlat, *_surfacePointsLessFlat, _sweepStamp, "/camera"); + publishCloudMsg(_pubLaserCloud, _laserCloud, _sweepStart, "/camera"); + publishCloudMsg(_pubCornerPointsSharp, _cornerPointsSharp, _sweepStart, "/camera"); + publishCloudMsg(_pubCornerPointsLessSharp, _cornerPointsLessSharp, _sweepStart, "/camera"); + publishCloudMsg(_pubSurfPointsFlat, _surfacePointsFlat, _sweepStart, "/camera"); + publishCloudMsg(_pubSurfPointsLessFlat, _surfacePointsLessFlat, _sweepStart, "/camera"); + // publish corresponding IMU transformation information - publishCloudMsg(_pubImuTrans, *_imuTrans, _sweepStamp, "/camera"); + _imuTrans[0].x = _imuStart.pitch.rad(); + _imuTrans[0].y = _imuStart.yaw.rad(); + _imuTrans[0].z = _imuStart.roll.rad(); + + _imuTrans[1].x = _imuCur.pitch.rad(); + _imuTrans[1].y = _imuCur.yaw.rad(); + _imuTrans[1].z = _imuCur.roll.rad(); + + Vector3 imuShiftFromStart = _imuPositionShift; + rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); + + _imuTrans[2].x = imuShiftFromStart.x(); + _imuTrans[2].y = imuShiftFromStart.y(); + _imuTrans[2].z = imuShiftFromStart.z(); + + Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity; + rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); + + _imuTrans[3].x = imuVelocityFromStart.x(); + _imuTrans[3].y = imuVelocityFromStart.y(); + _imuTrans[3].z = imuVelocityFromStart.z(); + + publishCloudMsg(_pubImuTrans, _imuTrans, _sweepStart, "/camera"); } } // end namespace loam diff --git a/src/lib/VelodyneScanRegistration.cpp b/src/lib/VelodyneScanRegistration.cpp index 84c457d..4d2dede 100644 --- a/src/lib/VelodyneScanRegistration.cpp +++ b/src/lib/VelodyneScanRegistration.cpp @@ -39,11 +39,12 @@ namespace loam { VelodyneScanRegistration::VelodyneScanRegistration(const float& scanPeriod, - const uint16_t& nScans, - const size_t& imuHistorySize, - const RegistrationParams& config) - : ScanRegistration(scanPeriod, nScans, imuHistorySize, config), - _systemDelay(20) + const uint16_t& nScanRings, + const RegistrationParams& config, + const size_t& imuHistorySize) + : ScanRegistration(scanPeriod, config, imuHistorySize), + _systemDelay(20), + _nScanRings(nScanRings) { }; @@ -58,14 +59,14 @@ bool VelodyneScanRegistration::setup(ros::NodeHandle& node, } _subLaserCloud = node.subscribe - ("/velodyne_points", 2, &VelodyneScanRegistration::processCloudMessage, this); + ("/velodyne_points", 2, &VelodyneScanRegistration::handleCloudMessage, this); return true; } -void VelodyneScanRegistration::processCloudMessage(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) +void VelodyneScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) { if (_systemDelay > 0) { _systemDelay--; @@ -84,31 +85,30 @@ void VelodyneScanRegistration::processCloudMessage(const sensor_msgs::PointCloud void VelodyneScanRegistration::process(const pcl::PointCloud& laserCloudIn, const ros::Time& scanTime) { - size_t cloudSize = laserCloudIn.points.size(); + size_t cloudSize = laserCloudIn.size(); // reset internal buffers and set IMU start state based on current scan time reset(scanTime); // determine scan start and end orientations - float startOri = -std::atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); - float endOri = -std::atan2(laserCloudIn.points[cloudSize - 1].y, - laserCloudIn.points[cloudSize - 1].x) + 2 * float(M_PI); + float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x); + float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y, + laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI); if (endOri - startOri > 3 * M_PI) { endOri -= 2 * M_PI; } else if (endOri - startOri < M_PI) { endOri += 2 * M_PI; } - size_t imuIdx = _imuStartIdx; bool halfPassed = false; pcl::PointXYZI point; - std::vector > laserCloudScans(_nScans); + std::vector > laserCloudScans(_nScanRings); // extract valid points from input cloud for (int i = 0; i < cloudSize; i++) { - point.x = laserCloudIn.points[i].y; - point.y = laserCloudIn.points[i].z; - point.z = laserCloudIn.points[i].x; + point.x = laserCloudIn[i].y; + point.y = laserCloudIn[i].z; + point.z = laserCloudIn[i].x; // skip NaN and INF valued points if (!pcl_isfinite(point.x) || @@ -125,8 +125,9 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las // calculate vertical point angle and scan ID float angle = rad2deg(std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z))); int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : 0.5)); - int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScans - 1); - if (scanID > (_nScans - 1) || scanID < 0 ){ + int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScanRings - 1); + + if (scanID >= _nScanRings || scanID < 0 ){ continue; } @@ -157,20 +158,9 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las point.intensity = scanID + relTime; // project point to the start of the sweep using corresponding IMU data - if (_imuHistory.size() > 0) { - while (imuIdx < _imuHistory.size() - 1 && (scanTime - _imuHistory[imuIdx].stamp).toSec() + relTime > 0) { - imuIdx++; - } - - if (imuIdx == 0 || (scanTime - _imuHistory[imuIdx].stamp).toSec() + relTime > 0) { - _imuCur = _imuHistory[imuIdx]; - } else { - float ratio = ((_imuHistory[imuIdx].stamp - scanTime).toSec() - relTime) - / (_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp).toSec(); - IMUState::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, _imuCur); - } - - transformToStartIMU(point, relTime); + if (hasIMUData()) { + setIMUTransformFor(relTime); + transformToStartIMU(point); } laserCloudScans[scanID].push_back(point); @@ -178,20 +168,18 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las // construct sorted full resolution cloud cloudSize = 0; - for (int i = 0; i < _nScans; i++) { - *_laserCloud += laserCloudScans[i]; + for (int i = 0; i < _nScanRings; i++) { + _laserCloud += laserCloudScans[i]; - _scanStartIndices[i] = cloudSize; - cloudSize += laserCloudScans[i].points.size(); - _scanEndIndices[i] = cloudSize - 1; + IndexRange range(cloudSize, 0); + cloudSize += laserCloudScans[i].size(); + range.second = cloudSize > 0 ? cloudSize - 1 : 0; + _scanIndices.push_back(range); } // extract features extractFeatures(); - // set final IMU transformation information - setIMUTrans(_scanPeriod); - // publish result publishResult(); } From 3ca101ad10d4d3aaac349a8295d8554ef8801135 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Mon, 4 Dec 2017 12:45:14 +0100 Subject: [PATCH 9/9] Some further project cleanup. * Moved Angle, Vector3 and Twist classes into separate header files and removed loam_types.h again. --- include/loam_velodyne/Angle.h | 70 ++++++++++++ include/loam_velodyne/LaserMapping.h | 2 +- include/loam_velodyne/LaserOdometry.h | 4 +- include/loam_velodyne/ScanRegistration.h | 3 +- include/loam_velodyne/Twist.h | 31 +++++ include/loam_velodyne/Vector3.h | 73 ++++++++++++ include/loam_velodyne/loam_types.h | 140 ----------------------- src/lib/math_utils.h | 3 +- 8 files changed, 181 insertions(+), 145 deletions(-) create mode 100644 include/loam_velodyne/Angle.h create mode 100644 include/loam_velodyne/Twist.h create mode 100644 include/loam_velodyne/Vector3.h delete mode 100644 include/loam_velodyne/loam_types.h diff --git a/include/loam_velodyne/Angle.h b/include/loam_velodyne/Angle.h new file mode 100644 index 0000000..4446e7e --- /dev/null +++ b/include/loam_velodyne/Angle.h @@ -0,0 +1,70 @@ +#ifndef LOAM_ANGLE_H +#define LOAM_ANGLE_H + + +#include + + +namespace loam { + + +/** \brief Class for holding an angle. + * + * This class provides buffered access to sine and cosine values to the represented angular value. + */ +class Angle { +public: + Angle() + : _radian(0.0), + _cos(1.0), + _sin(0.0) {} + + Angle(float radValue) + : _radian(radValue), + _cos(std::cos(radValue)), + _sin(std::sin(radValue)) {} + + Angle(const Angle &other) + : _radian(other._radian), + _cos(other._cos), + _sin(other._sin) {} + + void operator=(const Angle &rhs) { + _radian = (rhs._radian); + _cos = (rhs._cos); + _sin = (rhs._sin); + } + + void operator+=(const float &radValue) { *this = (_radian + radValue); } + + void operator+=(const Angle &other) { *this = (_radian + other._radian); } + + void operator-=(const float &radValue) { *this = (_radian - radValue); } + + void operator-=(const Angle &other) { *this = (_radian - other._radian); } + + Angle operator-() const { + Angle out; + out._radian = -_radian; + out._cos = _cos; + out._sin = -(_sin); + return out; + } + + float rad() const { return _radian; } + + float deg() const { return _radian * 180 / M_PI; } + + float cos() const { return _cos; } + + float sin() const { return _sin; } + +private: + float _radian; ///< angle value in radian + float _cos; ///< cosine of the angle + float _sin; ///< sine of the angle +}; + +} // end namespace loam + +#endif //LOAM_ANGLE_H diff --git a/include/loam_velodyne/LaserMapping.h b/include/loam_velodyne/LaserMapping.h index d86ed2f..e3e7354 100644 --- a/include/loam_velodyne/LaserMapping.h +++ b/include/loam_velodyne/LaserMapping.h @@ -34,7 +34,7 @@ #define LOAM_LASERMAPPING_H -#include "loam_types.h" +#include "Twist.h" #include "CircularBuffer.h" #include diff --git a/include/loam_velodyne/LaserOdometry.h b/include/loam_velodyne/LaserOdometry.h index aa44d6a..2405152 100644 --- a/include/loam_velodyne/LaserOdometry.h +++ b/include/loam_velodyne/LaserOdometry.h @@ -34,8 +34,8 @@ #define LOAM_LASERODOMETRY_H -#include "loam_types.h" -#include "loam_velodyne/nanoflann_pcl.h" +#include "Twist.h" +#include "nanoflann_pcl.h" #include #include diff --git a/include/loam_velodyne/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h index e86f8e6..c5d9053 100644 --- a/include/loam_velodyne/ScanRegistration.h +++ b/include/loam_velodyne/ScanRegistration.h @@ -35,7 +35,8 @@ #include "common.h" -#include "loam_types.h" +#include "Angle.h" +#include "Vector3.h" #include "CircularBuffer.h" #include diff --git a/include/loam_velodyne/Twist.h b/include/loam_velodyne/Twist.h new file mode 100644 index 0000000..8ddb1d4 --- /dev/null +++ b/include/loam_velodyne/Twist.h @@ -0,0 +1,31 @@ +#ifndef LOAM_TWIST_H +#define LOAM_TWIST_H + + +#include "Angle.h" +#include "Vector3.h" + + +namespace loam { + + +/** \brief Twist composed by three angles and a three-dimensional position. + * + */ +class Twist { +public: + Twist() + : rot_x(), + rot_y(), + rot_z(), + pos() {}; + + Angle rot_x; + Angle rot_y; + Angle rot_z; + Vector3 pos; +}; + +} // end namespace loam + +#endif //LOAM_TWIST_H diff --git a/include/loam_velodyne/Vector3.h b/include/loam_velodyne/Vector3.h new file mode 100644 index 0000000..e0657d2 --- /dev/null +++ b/include/loam_velodyne/Vector3.h @@ -0,0 +1,73 @@ +#ifndef LOAM_VECTOR3_H +#define LOAM_VECTOR3_H + + +#include + + +namespace loam { + +/** \brief Vector4f decorator for convenient handling. + * + */ +class Vector3 : public Eigen::Vector4f { +public: + Vector3(float x, float y, float z) + : Eigen::Vector4f(x, y, z, 0) {} + + Vector3(void) + : Eigen::Vector4f(0, 0, 0, 0) {} + + template + Vector3(const Eigen::MatrixBase &other) + : Eigen::Vector4f(other) {} + + Vector3(const pcl::PointXYZI &p) + : Eigen::Vector4f(p.x, p.y, p.z, 0) {} + + template + Vector3 &operator=(const Eigen::MatrixBase &rhs) { + this->Eigen::Vector4f::operator=(rhs); + return *this; + } + + Vector3 &operator=(const pcl::PointXYZ &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + Vector3 &operator=(const pcl::PointXYZI &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + float x() const { return (*this)(0); } + + float y() const { return (*this)(1); } + + float z() const { return (*this)(2); } + + float &x() { return (*this)(0); } + + float &y() { return (*this)(1); } + + float &z() { return (*this)(2); } + + // easy conversion + operator pcl::PointXYZI() { + pcl::PointXYZI dst; + dst.x = x(); + dst.y = y(); + dst.z = z(); + dst.intensity = 0; + return dst; + } +}; + +} // end namespace loam + +#endif //LOAM_VECTOR3_H diff --git a/include/loam_velodyne/loam_types.h b/include/loam_velodyne/loam_types.h deleted file mode 100644 index 1afee6c..0000000 --- a/include/loam_velodyne/loam_types.h +++ /dev/null @@ -1,140 +0,0 @@ -#ifndef LOAM_LOAM_TYPES_H -#define LOAM_LOAM_TYPES_H - - -#include - - -namespace loam { - -/** \brief Vector4f decorator for convenient handling. - * - */ -class Vector3 : public Eigen::Vector4f { -public: - Vector3(float x, float y, float z) - : Eigen::Vector4f(x, y, z, 0) {} - - Vector3(void) - : Eigen::Vector4f(0, 0, 0, 0) {} - - template - Vector3(const Eigen::MatrixBase &other) - : Eigen::Vector4f(other) {} - - Vector3(const pcl::PointXYZI &p) - : Eigen::Vector4f(p.x, p.y, p.z, 0) {} - - template - Vector3 &operator=(const Eigen::MatrixBase &rhs) { - this->Eigen::Vector4f::operator=(rhs); - return *this; - } - - Vector3 &operator=(const pcl::PointXYZ &rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - Vector3 &operator=(const pcl::PointXYZI &rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - float x() const { return (*this)(0); } - - float y() const { return (*this)(1); } - - float z() const { return (*this)(2); } - - float &x() { return (*this)(0); } - - float &y() { return (*this)(1); } - - float &z() { return (*this)(2); } - - // easy conversion - operator pcl::PointXYZI() { - pcl::PointXYZI dst; - dst.x = x(); - dst.y = y(); - dst.z = z(); - dst.intensity = 0; - return dst; - } -}; - - -/** \brief Class for holding an angle. - * - * This class provides buffered access to sine and cosine values to the represented angular value. - */ -class Angle { -public: - Angle() - : _radian(0.0), - _cos(1.0), - _sin(0.0) {} - - Angle(float radValue) - : _radian(radValue), - _cos(std::cos(radValue)), - _sin(std::sin(radValue)) {} - - Angle(const Angle &other) - : _radian(other._radian), - _cos(other._cos), - _sin(other._sin) {} - - void operator=(const Angle &rhs) { - _radian = (rhs._radian); - _cos = (rhs._cos); - _sin = (rhs._sin); - } - - void operator+=(const float &radValue) { *this = (_radian + radValue); } - - void operator+=(const Angle &other) { *this = (_radian + other._radian); } - - void operator-=(const float &radValue) { *this = (_radian - radValue); } - - void operator-=(const Angle &other) { *this = (_radian - other._radian); } - - Angle operator-() const { - Angle out; - out._radian = -_radian; - out._cos = _cos; - out._sin = -(_sin); - return out; - } - - float rad() const { return _radian; } - - float deg() const { return _radian * 180 / M_PI; } - - float cos() const { return _cos; } - - float sin() const { return _sin; } - -private: - float _radian; ///< angle value in radian - float _cos; ///< cosine of the angle - float _sin; ///< sine of the angle -}; - - -/** \brief Twist composed by three angles and a three-dimensional position. */ -struct Twist { - Angle rot_x; - Angle rot_y; - Angle rot_z; - Vector3 pos; -}; - -} // end namespace loam - -#endif //LOAM_LOAM_TYPES_H diff --git a/src/lib/math_utils.h b/src/lib/math_utils.h index 9e3651b..f6a3d37 100644 --- a/src/lib/math_utils.h +++ b/src/lib/math_utils.h @@ -2,7 +2,8 @@ #define LOAM_MATH_UTILS_H -#include "loam_velodyne/loam_types.h" +#include "loam_velodyne/Angle.h" +#include "loam_velodyne/Vector3.h" #include