diff --git a/.gitignore b/.gitignore index 7b4354fd..f9087b68 100755 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,10 @@ CMakeFiles cmake_install.cmake Makefile +.vscode +.tags* +.idea +.settings +*.VC.db +*.VC.opendb +*.suo diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f143b52..36bc420a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(loam_velodyne) +set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS}") + find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs diff --git a/include/loam_velodyne/common.h b/include/loam_velodyne/common.h index 67e5d877..c92dbfaa 100644 --- a/include/loam_velodyne/common.h +++ b/include/loam_velodyne/common.h @@ -45,3 +45,6 @@ inline double deg2rad(double degrees) { return degrees * M_PI / 180.0; } + +#define VELODYNE_HDL64E +#define OUTPUT_ALL_POINTCLOUD \ No newline at end of file diff --git a/package.xml b/package.xml index 9b1a98a8..36d7f3a0 100644 --- a/package.xml +++ b/package.xml @@ -36,6 +36,5 @@ rostest rosbag - - + diff --git a/rviz_cfg/loam_velodyne.rviz b/rviz_cfg/loam_velodyne.rviz index c9bcdd52..00b8aee9 100644 --- a/rviz_cfg/loam_velodyne.rviz +++ b/rviz_cfg/loam_velodyne.rviz @@ -181,8 +181,10 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/ThirdPersonFollower - Distance: 162.362 + // Class: rviz/ThirdPersonFollower + //Class: rviz/FPS + //Distance: 162.362 + Distance: 300 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 @@ -196,7 +198,8 @@ Visualization Manager: Near Clip Distance: 0.01 Pitch: -1.5698 Target Frame: aft_mapped - Value: ThirdPersonFollower (rviz) + //Value: ThirdPersonFollower (rviz) + Value: default Yaw: 4.29039 Saved: ~ Window Geometry: diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp old mode 100755 new mode 100644 index 472aa833..8d4ed007 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -30,26 +30,56 @@ // 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 #include #include #include -#include #include +#include + +struct FreqReport { + std::string name; + std::chrono::system_clock::time_point last_time; + bool firstTime; + FreqReport(const std::string &n) : name(n), firstTime(true) {} + void report() { + if (firstTime) { + firstTime = false; + last_time = std::chrono::system_clock::now(); + return; + } + auto cur_time = std::chrono::system_clock::now(); + ROS_INFO("time interval of %s = %f seconds\n", name.c_str(), + std::chrono::duration_cast< + std::chrono::duration>>(cur_time - + last_time) + .count()); + last_time = cur_time; + } +}; const float scanPeriod = 0.1; +#ifndef VELODYNE_HDL64E const int stackFrameNum = 1; const int mapFrameNum = 5; +const int maxIterNum = 10; +#else +const int stackFrameNum = 1; +const int mapFrameNum = 5; +const int maxIterNum = 20; +#endif + double timeLaserCloudCornerLast = 0; double timeLaserCloudSurfLast = 0; @@ -72,26 +102,39 @@ 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 + 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 + 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]; -pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN()); -pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); +pcl::KdTreeFLANN::Ptr + kdtreeCornerFromMap(new pcl::KdTreeFLANN()); +pcl::KdTreeFLANN::Ptr + kdtreeSurfFromMap(new pcl::KdTreeFLANN()); float transformSum[6] = {0}; float transformIncre[6] = {0}; @@ -107,13 +150,12 @@ double imuTime[imuQueLength] = {0}; float imuRoll[imuQueLength] = {0}; float imuPitch[imuQueLength] = {0}; -void transformAssociateToMap() -{ - float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); +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 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; @@ -144,58 +186,88 @@ void transformAssociateToMap() 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); + 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[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); - transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), + 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[1] = atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[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); - transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[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); + transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0])); - x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4]; - y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4]; + x1 = cos(transformTobeMapped[2]) * transformIncre[3] - + sin(transformTobeMapped[2]) * transformIncre[4]; + y1 = sin(transformTobeMapped[2]) * transformIncre[3] + + cos(transformTobeMapped[2]) * transformIncre[4]; z1 = transformIncre[5]; x2 = x1; y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; - transformTobeMapped[3] = transformAftMapped[3] - - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2); + transformTobeMapped[3] = + transformAftMapped[3] - + (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2); transformTobeMapped[4] = transformAftMapped[4] - y2; - transformTobeMapped[5] = transformAftMapped[5] - - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2); + transformTobeMapped[5] = + transformAftMapped[5] - + (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2); } -void transformUpdate() -{ +void transformUpdate() { if (imuPointerLast >= 0) { float imuRollLast = 0, imuPitchLast = 0; while (imuPointerFront != imuPointerLast) { @@ -210,17 +282,23 @@ void transformUpdate() 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; + 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[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast; - transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast; + transformTobeMapped[0] = + 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast; + transformTobeMapped[2] = + 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast; } for (int i = 0; i < 6; i++) { @@ -229,48 +307,48 @@ void transformUpdate() } } -void pointAssociateToMap(PointType const * const pi, PointType * const po) -{ - float x1 = cos(transformTobeMapped[2]) * pi->x - - sin(transformTobeMapped[2]) * pi->y; - float y1 = sin(transformTobeMapped[2]) * pi->x - + cos(transformTobeMapped[2]) * pi->y; +void pointAssociateToMap(PointType const *const pi, PointType *const po) { + float x1 = + cos(transformTobeMapped[2]) * pi->x - sin(transformTobeMapped[2]) * pi->y; + float y1 = + sin(transformTobeMapped[2]) * pi->x + cos(transformTobeMapped[2]) * pi->y; float z1 = pi->z; float x2 = x1; - float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; - float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; + float y2 = + cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; + float z2 = + sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; - po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2 - + transformTobeMapped[3]; + po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2 + + transformTobeMapped[3]; po->y = y2 + transformTobeMapped[4]; - po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2 - + transformTobeMapped[5]; + po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2 + + transformTobeMapped[5]; po->intensity = pi->intensity; } -void pointAssociateTobeMapped(PointType const * const pi, PointType * const po) -{ - float x1 = cos(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) - - sin(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); +void pointAssociateTobeMapped(PointType const *const pi, PointType *const po) { + float x1 = cos(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) - + sin(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); float y1 = pi->y - transformTobeMapped[4]; - float z1 = sin(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) - + cos(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); + float z1 = sin(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) + + cos(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); float x2 = x1; - float y2 = cos(transformTobeMapped[0]) * y1 + sin(transformTobeMapped[0]) * z1; - float z2 = -sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; + float y2 = + cos(transformTobeMapped[0]) * y1 + sin(transformTobeMapped[0]) * z1; + float z2 = + -sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; - po->x = cos(transformTobeMapped[2]) * x2 - + sin(transformTobeMapped[2]) * y2; - po->y = -sin(transformTobeMapped[2]) * x2 - + cos(transformTobeMapped[2]) * y2; + po->x = cos(transformTobeMapped[2]) * x2 + sin(transformTobeMapped[2]) * y2; + po->y = -sin(transformTobeMapped[2]) * x2 + cos(transformTobeMapped[2]) * y2; po->z = z2; po->intensity = pi->intensity; } -void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2) -{ +void laserCloudCornerLastHandler( + const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2) { timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec(); laserCloudCornerLast->clear(); @@ -279,8 +357,8 @@ void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCl newLaserCloudCornerLast = true; } -void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2) -{ +void laserCloudSurfLastHandler( + const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2) { timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec(); laserCloudSurfLast->clear(); @@ -289,8 +367,8 @@ void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserClou newLaserCloudSurfLast = true; } -void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) -{ +void laserCloudFullResHandler( + const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) { timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); laserCloudFullRes->clear(); @@ -299,13 +377,13 @@ void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloud newLaserCloudFullRes = true; } -void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) -{ +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); + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) + .getRPY(roll, pitch, yaw); transformSum[0] = -pitch; transformSum[1] = -yaw; @@ -318,8 +396,7 @@ void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) newLaserOdometry = true; } -void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) -{ +void imuHandler(const sensor_msgs::Imu::ConstPtr &imuIn) { double roll, pitch, yaw; tf::Quaternion orientation; tf::quaternionMsgToTF(imuIn->orientation, orientation); @@ -332,32 +409,38 @@ void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) imuPitch[imuPointerLast] = pitch; } -int main(int argc, char** argv) -{ +FreqReport mappingCloudFreq("mappingCloud"); +FreqReport mappingOdometryFreq("mappingOdometry"); + +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 subLaserCloudCornerLast = + nh.subscribe("/laser_cloud_corner_last", 2, + laserCloudCornerLastHandler); - ros::Subscriber subLaserCloudSurfLast = nh.subscribe - ("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler); + ros::Subscriber subLaserCloudSurfLast = + nh.subscribe("/laser_cloud_surf_last", 2, + laserCloudSurfLastHandler); - ros::Subscriber subLaserOdometry = nh.subscribe - ("/laser_odom_to_init", 5, laserOdometryHandler); + ros::Subscriber subLaserOdometry = nh.subscribe( + "/laser_odom_to_init", 5, laserOdometryHandler); - ros::Subscriber subLaserCloudFullRes = nh.subscribe - ("/velodyne_cloud_3", 2, laserCloudFullResHandler); + ros::Subscriber subLaserCloudFullRes = nh.subscribe( + "/velodyne_cloud_3", 2, laserCloudFullResHandler); - ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); + ros::Subscriber subImu = + nh.subscribe("/imu/data", 50, imuHandler); - ros::Publisher pubLaserCloudSurround = nh.advertise - ("/laser_cloud_surround", 1); + ros::Publisher pubLaserCloudSurround = + nh.advertise("/laser_cloud_surround", 1); - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/velodyne_cloud_registered", 2); + ros::Publisher pubLaserCloudFullRes = + nh.advertise("/velodyne_cloud_registered", 2); - ros::Publisher pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 5); + 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"; @@ -406,7 +489,8 @@ int main(int argc, char** argv) while (status) { ros::spinOnce(); - if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry && + if (newLaserCloudCornerLast && newLaserCloudSurfLast && + newLaserCloudFullRes && newLaserOdometry && fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 && fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 && fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) { @@ -441,32 +525,49 @@ int main(int argc, char** argv) pointOnYAxis.z = 0.0; pointAssociateToMap(&pointOnYAxis, &pointOnYAxis); - int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth; - int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight; - int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth; + int centerCubeI = + int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth; + int centerCubeJ = + int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight; + int centerCubeK = + int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth; + + if (transformTobeMapped[3] + 25.0 < 0) + centerCubeI--; + if (transformTobeMapped[4] + 25.0 < 0) + centerCubeJ--; + if (transformTobeMapped[5] + 25.0 < 0) + centerCubeK--; - if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--; - if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--; - if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--; + // shift block of point cloud 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]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeCornerPointer; + laserCloudSurfArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } @@ -481,19 +582,28 @@ int main(int argc, char** argv) for (int k = 0; k < laserCloudDepth; k++) { int i = 0; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeCornerPointer; + laserCloudSurfArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } @@ -508,46 +618,64 @@ int main(int argc, char** argv) for (int k = 0; k < laserCloudDepth; k++) { int j = laserCloudHeight - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + 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]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeCornerPointer; + laserCloudSurfArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } @@ -562,19 +690,29 @@ int main(int argc, char** argv) for (int j = 0; j < laserCloudHeight; j++) { int k = laserCloudDepth - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = - laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeCornerPointer; + laserCloudSurfArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } @@ -583,25 +721,35 @@ int main(int argc, char** argv) 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]; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer = - laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; + 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] = + 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; + laserCloudCornerArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeCornerPointer; + laserCloudSurfArray[i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k] = + laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } @@ -616,9 +764,8 @@ int main(int argc, char** argv) 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) { + 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); @@ -632,22 +779,26 @@ int main(int argc, char** argv) float cornerY = centerY + 25.0 * jj; float cornerZ = centerZ + 25.0 * kk; - float squaredSide1 = (transformTobeMapped[3] - cornerX) - * (transformTobeMapped[3] - cornerX) - + (transformTobeMapped[4] - cornerY) - * (transformTobeMapped[4] - cornerY) - + (transformTobeMapped[5] - cornerZ) - * (transformTobeMapped[5] - cornerZ); + float squaredSide1 = + (transformTobeMapped[3] - cornerX) * + (transformTobeMapped[3] - cornerX) + + (transformTobeMapped[4] - cornerY) * + (transformTobeMapped[4] - cornerY) + + (transformTobeMapped[5] - cornerZ) * + (transformTobeMapped[5] - cornerZ); - float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) - + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY) - + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ); + float squaredSide2 = (pointOnYAxis.x - cornerX) * + (pointOnYAxis.x - cornerX) + + (pointOnYAxis.y - cornerY) * + (pointOnYAxis.y - cornerY) + + (pointOnYAxis.z - cornerZ) * + (pointOnYAxis.z - cornerZ); - float check1 = 100.0 + squaredSide1 - squaredSide2 - - 10.0 * sqrt(3.0) * sqrt(squaredSide1); + 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); + float check2 = 100.0 + squaredSide1 - squaredSide2 + + 10.0 * sqrt(3.0) * sqrt(squaredSide1); if (check1 < 0 && check2 > 0) { isInLaserFOV = true; @@ -657,12 +808,14 @@ int main(int argc, char** argv) } if (isInLaserFOV) { - laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j - + laserCloudWidth * laserCloudHeight * k; + laserCloudValidInd[laserCloudValidNum] = + i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum++; } - laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j - + laserCloudWidth * laserCloudHeight * k; + laserCloudSurroundInd[laserCloudSurroundNum] = + i + laserCloudWidth * j + + laserCloudWidth * laserCloudHeight * k; laserCloudSurroundNum++; } } @@ -672,7 +825,8 @@ int main(int argc, char** argv) laserCloudCornerFromMap->clear(); laserCloudSurfFromMap->clear(); for (int i = 0; i < laserCloudValidNum; i++) { - *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; + *laserCloudCornerFromMap += + *laserCloudCornerArray[laserCloudValidInd[i]]; *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; } int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size(); @@ -680,12 +834,14 @@ int main(int argc, char** argv) int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size(); for (int i = 0; i < laserCloudCornerStackNum2; i++) { - pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[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]); + pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], + &laserCloudSurfStack2->points[i]); } laserCloudCornerStack->clear(); @@ -705,18 +861,20 @@ int main(int argc, char** argv) kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); - for (int iterCount = 0; iterCount < 10; iterCount++) { + for (int iterCount = 0; iterCount < maxIterNum; iterCount++) { laserCloudOri->clear(); coeffSel->clear(); + // find correspondance for (int i = 0; i < laserCloudCornerStackNum; i++) { pointOri = laserCloudCornerStack->points[i]; pointAssociateToMap(&pointOri, &pointSel); - kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); - + kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, + pointSearchSqDis); + if (pointSearchSqDis[4] < 1.0) { float cx = 0; - float cy = 0; + float cy = 0; float cz = 0; for (int j = 0; j < 5; j++) { cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x; @@ -724,19 +882,22 @@ int main(int argc, char** argv) cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z; } cx /= 5; - cy /= 5; + cy /= 5; cz /= 5; float a11 = 0; - float a12 = 0; + float a12 = 0; float a13 = 0; float a22 = 0; - float a23 = 0; + float a23 = 0; float a33 = 0; for (int j = 0; j < 5; j++) { - float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx; - float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy; - float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz; + float ax = + laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx; + float ay = + laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy; + float az = + laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz; a11 += ax * ax; a12 += ax * ay; @@ -746,10 +907,10 @@ int main(int argc, char** argv) a33 += az * az; } a11 /= 5; - a12 /= 5; + a12 /= 5; a13 /= 5; a22 /= 5; - a23 /= 5; + a23 /= 5; a33 /= 5; matA1.at(0, 0) = a11; @@ -776,23 +937,38 @@ int main(int argc, char** argv) float y2 = cy - 0.1 * matV1.at(0, 1); float z2 = cz - 0.1 * matV1.at(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 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; @@ -815,17 +991,21 @@ int main(int argc, char** argv) } } } - + // find correspondance for (int i = 0; i < laserCloudSurfStackNum; i++) { pointOri = laserCloudSurfStack->points[i]; - pointAssociateToMap(&pointOri, &pointSel); - kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); + pointAssociateToMap(&pointOri, &pointSel); + kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, + pointSearchSqDis); if (pointSearchSqDis[4] < 1.0) { for (int j = 0; j < 5; j++) { - matA0.at(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; - matA0.at(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; - matA0.at(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; + matA0.at(j, 0) = + laserCloudSurfFromMap->points[pointSearchInd[j]].x; + matA0.at(j, 1) = + laserCloudSurfFromMap->points[pointSearchInd[j]].y; + matA0.at(j, 2) = + laserCloudSurfFromMap->points[pointSearchInd[j]].z; } cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); @@ -833,7 +1013,7 @@ int main(int argc, char** argv) float pb = matX0.at(1, 0); float pc = matX0.at(2, 0); float pd = 1; - + float ps = sqrt(pa * pa + pb * pb + pc * pc); pa /= ps; pb /= ps; @@ -842,24 +1022,35 @@ int main(int argc, char** argv) 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) { + 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; + 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)); + 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; @@ -896,18 +1087,34 @@ int main(int argc, char** argv) 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; + 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.at(i, 0) = arx; matA.at(i, 1) = ary; @@ -958,12 +1165,10 @@ int main(int argc, char** argv) transformTobeMapped[4] += matX.at(4, 0); transformTobeMapped[5] += matX.at(5, 0); - float deltaR = sqrt( - pow(rad2deg(matX.at(0, 0)), 2) + + float deltaR = sqrt(pow(rad2deg(matX.at(0, 0)), 2) + pow(rad2deg(matX.at(1, 0)), 2) + pow(rad2deg(matX.at(2, 0)), 2)); - float deltaT = sqrt( - pow(matX.at(3, 0) * 100, 2) + + float deltaT = sqrt(pow(matX.at(3, 0) * 100, 2) + pow(matX.at(4, 0) * 100, 2) + pow(matX.at(5, 0) * 100, 2)); @@ -982,14 +1187,18 @@ int main(int argc, char** argv) 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; + 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); } } @@ -1001,14 +1210,18 @@ int main(int argc, char** argv) 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; + 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); } } @@ -1024,7 +1237,8 @@ int main(int argc, char** argv) downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]); downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]); - pcl::PointCloud::Ptr laserCloudTemp = laserCloudCornerArray[ind]; + pcl::PointCloud::Ptr laserCloudTemp = + laserCloudCornerArray[ind]; laserCloudCornerArray[ind] = laserCloudCornerArray2[ind]; laserCloudCornerArray2[ind] = laserCloudTemp; @@ -1038,11 +1252,19 @@ int main(int argc, char** argv) mapFrameCount = 0; laserCloudSurround2->clear(); + + #ifdef OUTPUT_ALL_POINTCLOUD + for (int i = 0; i < laserCloudNum; i++) { + *laserCloudSurround2 += *laserCloudCornerArray[i]; + *laserCloudSurround2 += *laserCloudSurfArray[i]; + } + #else for (int i = 0; i < laserCloudSurroundNum; i++) { int ind = laserCloudSurroundInd[i]; *laserCloudSurround2 += *laserCloudCornerArray[ind]; *laserCloudSurround2 += *laserCloudSurfArray[ind]; } + #endif laserCloudSurround->clear(); downSizeFilterCorner.setInputCloud(laserCloudSurround2); @@ -1050,24 +1272,31 @@ int main(int argc, char** argv) sensor_msgs::PointCloud2 laserCloudSurround3; pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3); - laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry); + laserCloudSurround3.header.stamp = + ros::Time().fromSec(timeLaserOdometry); laserCloudSurround3.header.frame_id = "/camera_init"; pubLaserCloudSurround.publish(laserCloudSurround3); + + // mappingCloudFreq.report(); } int laserCloudFullResNum = laserCloudFullRes->points.size(); for (int i = 0; i < laserCloudFullResNum; i++) { - pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[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.stamp = + ros::Time().fromSec(timeLaserOdometry); laserCloudFullRes3.header.frame_id = "/camera_init"; pubLaserCloudFullRes.publish(laserCloudFullRes3); - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw - (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]); + geometry_msgs::Quaternion geoQuat = + tf::createQuaternionMsgFromRollPitchYaw(transformAftMapped[2], + -transformAftMapped[0], + -transformAftMapped[1]); odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); odomAftMapped.pose.pose.orientation.x = -geoQuat.y; @@ -1085,12 +1314,15 @@ int main(int argc, char** argv) odomAftMapped.twist.twist.linear.z = transformBefMapped[5]; pubOdomAftMapped.publish(odomAftMapped); + // mappingOdometryFreq.report(); + aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry); - aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], - transformAftMapped[4], transformAftMapped[5])); + aftMappedTrans.setRotation( + tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], + transformAftMapped[4], + transformAftMapped[5])); tfBroadcaster.sendTransform(aftMappedTrans); - } } @@ -1100,4 +1332,3 @@ int main(int argc, char** argv) return 0; } - diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp index 4a48c891..ff3967fd 100755 --- a/src/laserOdometry.cpp +++ b/src/laserOdometry.cpp @@ -30,25 +30,55 @@ // 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 #include #include -#include #include +#include + +struct FreqReport { + std::string name; + std::chrono::system_clock::time_point last_time; + bool firstTime; + FreqReport(const std::string &n) : name(n), firstTime(true) {} + void report() { + if (firstTime) { + firstTime = false; + last_time = std::chrono::system_clock::now(); + return; + } + auto cur_time = std::chrono::system_clock::now(); + ROS_INFO("time interval of %s = %f seconds\n", name.c_str(), + std::chrono::duration_cast< + std::chrono::duration>>(cur_time - + last_time) + .count()); + last_time = cur_time; + } +}; const float scanPeriod = 0.1; + +#ifndef VELODYNE_HDL64E const int skipFrameNum = 1; +const int maxIterNum = 25; +#else +const int skipFrameNum = 1; +const int maxIterNum = 100; +#endif + bool systemInited = false; double timeCornerPointsSharp = 0; @@ -65,42 +95,64 @@ 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 + 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()); // stores the original + // coordinates (not + // transformed to start + // time point) of feature + // point in current cloud pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); -pcl::PointCloud::Ptr imuTrans(new pcl::PointCloud()); -pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); -pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); +pcl::PointCloud::Ptr + laserCloudFullRes(new pcl::PointCloud()); +pcl::PointCloud::Ptr + imuTrans(new pcl::PointCloud()); +pcl::KdTreeFLANN::Ptr + kdtreeCornerLast(new pcl::KdTreeFLANN()); +pcl::KdTreeFLANN::Ptr + kdtreeSurfLast(new pcl::KdTreeFLANN()); int laserCloudCornerLastNum; int laserCloudSurfLastNum; -int pointSelCornerInd[40000]; -float pointSearchCornerInd1[40000]; -float pointSearchCornerInd2[40000]; +#ifndef VELODYNE_HDL64E +const int MAX_POINTS = 40000; +#else +const int MAX_POINTS = 160000; +#endif + +int pointSelCornerInd[MAX_POINTS]; +float pointSearchCornerInd1[MAX_POINTS]; +float pointSearchCornerInd2[MAX_POINTS]; -int pointSelSurfInd[40000]; -float pointSearchSurfInd1[40000]; -float pointSearchSurfInd2[40000]; -float pointSearchSurfInd3[40000]; +int pointSelSurfInd[MAX_POINTS]; +float pointSearchSurfInd1[MAX_POINTS]; +float pointSearchSurfInd2[MAX_POINTS]; +float pointSearchSurfInd3[MAX_POINTS]; -float transform[6] = {0}; -float transformSum[6] = {0}; +float transform[6] = { + 0}; // rotation {x, y, z} or {pitch?, yaw?, roll?}, translation {x, y, z} +float transformSum[6] = { + 0}; // rotation {x, y, z} or {pitch?, yaw?, roll?}, translation {x, y, z} float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0; float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0; float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0; -void TransformToStart(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); +void TransformToStart(PointType const *const pi, PointType *const po) { + float s = 10 * (pi->intensity - int(pi->intensity)); // interpolation factor float rx = s * transform[0]; float ry = s * transform[1]; @@ -123,9 +175,8 @@ void TransformToStart(PointType const * const pi, PointType * const po) po->intensity = pi->intensity; } -void TransformToEnd(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); +void TransformToEnd(PointType const *const pi, PointType *const po) { + float s = 10 * (pi->intensity - int(pi->intensity)); // interpolation factor float rx = s * transform[0]; float ry = s * transform[1]; @@ -165,10 +216,10 @@ void TransformToEnd(PointType const * const pi, PointType * const po) float y6 = sin(rz) * x5 + cos(rz) * y5 + ty; float z6 = z5 + tz; - float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) - - sin(imuRollStart) * (y6 - imuShiftFromStartY); - float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) - + cos(imuRollStart) * (y6 - imuShiftFromStartY); + float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) - + sin(imuRollStart) * (y6 - imuShiftFromStartY); + float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) + + cos(imuRollStart) * (y6 - imuShiftFromStartY); float z7 = z6 - imuShiftFromStartZ; float x8 = x7; @@ -193,9 +244,9 @@ void TransformToEnd(PointType const * const pi, PointType * const po) po->intensity = int(pi->intensity); } -void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, - float alx, float aly, float alz, float &acx, float &acy, float &acz) -{ +void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, + float blz, float alx, float aly, float alz, float &acx, + float &acy, float &acz) { float sbcx = sin(bcx); float cbcx = cos(bcx); float sbcy = sin(bcy); @@ -217,118 +268,152 @@ void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, fl float salz = sin(alz); float calz = cos(alz); - 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); + 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); + 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 / cos(acx), crycrx / cos(acx)); - - 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); + + 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 / cos(acx), crzcrx / cos(acx)); } -void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, - float &ox, float &oy, float &oz) -{ - float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx); +void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, + float lz, float &ox, float &oy, float &oz) { + float srx = cos(lx) * cos(cx) * sin(ly) * sin(cz) - + cos(cx) * cos(cz) * sin(lx) - cos(lx) * cos(ly) * sin(cx); ox = -asin(srx); - float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) - + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy); - float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx)); + float srycrx = + sin(lx) * (cos(cy) * sin(cz) - cos(cz) * sin(cx) * sin(cy)) + + cos(lx) * sin(ly) * (cos(cy) * cos(cz) + sin(cx) * sin(cy) * sin(cz)) + + cos(lx) * cos(ly) * cos(cx) * sin(cy); + float crycrx = + cos(lx) * cos(ly) * cos(cx) * cos(cy) - + cos(lx) * sin(ly) * (cos(cz) * sin(cy) - cos(cy) * sin(cx) * sin(cz)) - + sin(lx) * (sin(cy) * sin(cz) + cos(cy) * cos(cz) * sin(cx)); oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); - float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) - + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz); - float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)); + float srzcrx = + sin(cx) * (cos(lz) * sin(ly) - cos(ly) * sin(lx) * sin(lz)) + + cos(cx) * sin(cz) * (cos(ly) * cos(lz) + sin(lx) * sin(ly) * sin(lz)) + + cos(lx) * cos(cx) * cos(cz) * sin(lz); + float crzcrx = + cos(lx) * cos(lz) * cos(cx) * cos(cz) - + cos(cx) * sin(cz) * (cos(ly) * sin(lz) - cos(lz) * sin(lx) * sin(ly)) - + sin(cx) * (sin(ly) * sin(lz) + cos(ly) * cos(lz) * sin(lx)); oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); } -void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2) -{ +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); + pcl::removeNaNFromPointCloud(*cornerPointsSharp, *cornerPointsSharp, indices); newCornerPointsSharp = true; } -void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2) -{ +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); + pcl::removeNaNFromPointCloud(*cornerPointsLessSharp, *cornerPointsLessSharp, + indices); newCornerPointsLessSharp = true; } -void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2) -{ +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); + pcl::removeNaNFromPointCloud(*surfPointsFlat, *surfPointsFlat, indices); newSurfPointsFlat = true; } -void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2) -{ +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); + pcl::removeNaNFromPointCloud(*surfPointsLessFlat, *surfPointsLessFlat, + indices); newSurfPointsLessFlat = true; } -void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) -{ +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); + pcl::removeNaNFromPointCloud(*laserCloudFullRes, *laserCloudFullRes, indices); newLaserCloudFullRes = true; } -void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) -{ +void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr &imuTrans2) { timeImuTrans = imuTrans2->header.stamp.toSec(); imuTrans->clear(); @@ -353,40 +438,43 @@ void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) newImuTrans = true; } - -int main(int argc, char** argv) -{ +FreqReport laserOdometryFreq("laserOdometry"); +FreqReport registeredLaserCloudFreq("registeredLaserCloud"); +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 subCornerPointsSharp = nh.subscribe( + "/laser_cloud_sharp", 2, laserCloudSharpHandler); - ros::Subscriber subCornerPointsLessSharp = nh.subscribe - ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler); + ros::Subscriber subCornerPointsLessSharp = + nh.subscribe("/laser_cloud_less_sharp", 2, + laserCloudLessSharpHandler); - ros::Subscriber subSurfPointsFlat = nh.subscribe - ("/laser_cloud_flat", 2, laserCloudFlatHandler); + ros::Subscriber subSurfPointsFlat = nh.subscribe( + "/laser_cloud_flat", 2, laserCloudFlatHandler); - ros::Subscriber subSurfPointsLessFlat = nh.subscribe - ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler); + ros::Subscriber subSurfPointsLessFlat = + nh.subscribe("/laser_cloud_less_flat", 2, + laserCloudLessFlatHandler); - ros::Subscriber subLaserCloudFullRes = nh.subscribe - ("/velodyne_cloud_2", 2, laserCloudFullResHandler); + ros::Subscriber subLaserCloudFullRes = nh.subscribe( + "/velodyne_cloud_2", 2, laserCloudFullResHandler); - ros::Subscriber subImuTrans = nh.subscribe - ("/imu_trans", 5, imuTransHandler); + ros::Subscriber subImuTrans = + nh.subscribe("/imu_trans", 5, imuTransHandler); - ros::Publisher pubLaserCloudCornerLast = nh.advertise - ("/laser_cloud_corner_last", 2); + ros::Publisher pubLaserCloudCornerLast = + nh.advertise("/laser_cloud_corner_last", 2); - ros::Publisher pubLaserCloudSurfLast = nh.advertise - ("/laser_cloud_surf_last", 2); + ros::Publisher pubLaserCloudSurfLast = + nh.advertise("/laser_cloud_surf_last", 2); - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/velodyne_cloud_3", 2); + ros::Publisher pubLaserCloudFullRes = + nh.advertise("/velodyne_cloud_3", 2); - ros::Publisher pubLaserOdometry = nh.advertise ("/laser_odom_to_init", 5); + 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"; @@ -399,7 +487,13 @@ int main(int argc, char** argv) std::vector pointSearchInd; std::vector pointSearchSqDis; - PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff; + // pointOri stores the original coordinates (not transformed to start time + // point) of feature point in current cloud + // coeff.xyz stores step * diff(distance(pointSel, {edge/plane}), {pointSel.x, + // pointSel.y, pointSel.z}), diff means gradient + // coeff.instance stores step * distance(pointSel, {edge/plane}) + PointType pointOri, pointSel, tripod1, tripod2, tripod3, + /*pointProj, */ coeff; bool isDegenerate = false; cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); @@ -410,7 +504,7 @@ int main(int argc, char** argv) while (status) { ros::spinOnce(); - if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && + if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans && fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 && fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 && @@ -425,94 +519,128 @@ int main(int argc, char** argv) newImuTrans = false; if (!systemInited) { + // initialize the "last clouds" & kdtrees, publish the first clouds, + // initialize the pitch and roll components of transformSum + + // initialize laserCloudCornerLast pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; - + // initialize laserCloudSurfLast laserCloudTemp = surfPointsLessFlat; surfPointsLessFlat = laserCloudSurfLast; laserCloudSurfLast = laserCloudTemp; - + // initialize kdtreeCornerLast kdtreeSurfLast kdtreeCornerLast->setInputCloud(laserCloudCornerLast); kdtreeSurfLast->setInputCloud(laserCloudSurfLast); - + // publish the first laserCloudCornerLast sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); + laserCloudCornerLast2.header.stamp = + ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudCornerLast2.header.frame_id = "/camera"; pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - + // publish the first laserCloudSurfLast2 sensor_msgs::PointCloud2 laserCloudSurfLast2; pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); + laserCloudSurfLast2.header.stamp = + ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudSurfLast2.header.frame_id = "/camera"; pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - transformSum[0] += imuPitchStart; - transformSum[2] += imuRollStart; + transformSum[0] += imuPitchStart; // TODO + transformSum[2] += imuRollStart; // TODO systemInited = true; continue; } + // minus the predicted motion transform[3] -= imuVeloFromStartX * scanPeriod; transform[4] -= imuVeloFromStartY * scanPeriod; transform[5] -= imuVeloFromStartZ * scanPeriod; - if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { + if (laserCloudCornerLastNum > 10 && + laserCloudSurfLastNum > + 100) { // when features are sufficient in last cloud std::vector indices; - pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); + pcl::removeNaNFromPointCloud(*cornerPointsSharp, *cornerPointsSharp, + indices); + int cornerPointsSharpNum = cornerPointsSharp->points.size(); int surfPointsFlatNum = surfPointsFlat->points.size(); - for (int iterCount = 0; iterCount < 25; iterCount++) { + + for (int iterCount = 0; iterCount < maxIterNum; iterCount++) { laserCloudOri->clear(); coeffSel->clear(); for (int i = 0; i < cornerPointsSharpNum; i++) { + // transform current point to the frame of start time point TransformToStart(&cornerPointsSharp->points[i], &pointSel); - if (iterCount % 5 == 0) { + if (iterCount % 5 == 0) { // locate the nearest point in last corner + // points to this cornerPointsSharp point + // every 5 iters std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices); - kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); + pcl::removeNaNFromPointCloud(*laserCloudCornerLast, + *laserCloudCornerLast, indices); + kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, + pointSearchSqDis); int closestPointInd = -1, minPointInd2 = -1; - if (pointSearchSqDis[0] < 25) { + if (pointSearchSqDis[0] < 25) { // when distance to the found + // nearest point pointSearchInd[0] + // in last cloud be < 5 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) { + int closestPointScan = int( + laserCloudCornerLast->points[closestPointInd] + .intensity); // get the scan line id of closestPointInd + + float pointSqDis, minPointSqDis2 = 25; // max distance: 5 + for (int j = closestPointInd + 1; j < cornerPointsSharpNum; + j++) { // search forward points, find the other point in + // last cloud + // TODO: j is bounded by cornerPointsSharpNum, can it be used + // to index laserCloudCornerLast? + if (int(laserCloudCornerLast->points[j].intensity) > + closestPointScan + 2.5) { 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); - - if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) { + // squared distance between the other point and pointSel + 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); + + if (int(laserCloudCornerLast->points[j].intensity) > + closestPointScan) { // the other point must not be in the + // same scan if (pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } } } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) { + for (int j = closestPointInd - 1; j >= 0; + j--) { // search backward points + if (int(laserCloudCornerLast->points[j].intensity) < + closestPointScan - 2.5) { 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); - - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) { + // squared distance between the other point and pointSel + 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); + + if (int(laserCloudCornerLast->points[j].intensity) < + closestPointScan) { // the other point must not be in the + // same scan if (pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; @@ -521,11 +649,15 @@ int main(int argc, char** argv) } } - pointSearchCornerInd1[i] = closestPointInd; - pointSearchCornerInd2[i] = minPointInd2; + pointSearchCornerInd1[i] = + closestPointInd; // the first point in last cloud closest to + // pointSel (distance < 5) + pointSearchCornerInd2[i] = + minPointInd2; // the second point in neaby scan within last + // clound closest to pointSel (distance < 5) } - if (pointSearchCornerInd2[i] >= 0) { + if (pointSearchCornerInd2[i] >= 0) { // found all two closest points tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]]; tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]]; @@ -539,34 +671,55 @@ int main(int argc, char** argv) 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; + // cross(pointSel - tripod1, pointSel - tripod2) = + // [(y0 - y1) (z0 - z2) - (y0 - y2) (z0 - z1), + // (x0 - x2) (z0 - z1) - (x0 - x1) (z0 - z2), + // (x0 - x1) (y0 - y2) - (x0 - x2) (y0 - y1)] + // a012 = |cross(pointSel - tripod1, pointSel - tripod2)| + 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))); + + // l12 = |tripod1 - tripod2| + float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + + (z1 - z2) * (z1 - z2)); + + // diff(ld2, x0), ld2 is the distance from pointSel to edge + // (tripod1, tripod2) as defined below + float la = + ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + + (z1 - z2) * + ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / + a012 / l12; + // diff(ld2, y0) + float lb = -((x1 - x2) * + ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) - + (z1 - z2) * ((y0 - y1) * (z0 - z2) - + (y0 - y2) * (z0 - z1))) / + a012 / l12; + // diff(ld2, z0) + 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; // distance from pointSel to edge (tripod1, tripod2) + + // pointProj = pointSel; + // pointProj.x -= la * ld2; + // pointProj.y -= lb * ld2; + // pointProj.z -= lc * ld2; + + float s = 1; // TODO: step? weight? if (iterCount >= 5) { - s = 1 - 1.8 * fabs(ld2); + s = 1 - 1.8 * fabs(ld2); // TODO: why adjust s like this? } coeff.x = s * la; @@ -574,7 +727,9 @@ int main(int argc, char** argv) coeff.z = s * lc; coeff.intensity = s * ld2; - if (s > 0.1 && ld2 != 0) { + if (s > 0.1 && ld2 != 0) { // apply this correspondence only when + // s is not too small and distance is + // not zero laserCloudOri->push_back(cornerPointsSharp->points[i]); coeffSel->push_back(coeff); } @@ -584,56 +739,79 @@ int main(int argc, char** argv) 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) { + if (iterCount % 5 == + 0) { // time to find correspondence with planar patches + kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, + pointSearchSqDis); + int closestPointInd = -1, minPointInd2 = -1, + minPointInd3 = -1; // another point closest to closestPointInd + // [minPointInd2->within] / + // [minPointInd3->not within] same scan + if (pointSearchSqDis[0] < 25) { // when distance to the found + // nearest point pointSearchInd[0] + // in last cloud be < 5 closestPointInd = pointSearchInd[0]; - int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity); + int closestPointScan = int( + laserCloudSurfLast->points[closestPointInd] + .intensity); // get the scan line id of closestPointInd float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; - for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { - if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) { + for (int j = closestPointInd + 1; j < surfPointsFlatNum; + j++) { // search forward points, find the other point in + // last cloud + // TODO: j is bounded by surfPointsFlatNum, can it be used to + // index laserCloudSurfLast? + if (int(laserCloudSurfLast->points[j].intensity) > + closestPointScan + 2.5) { // search another point within + // 2.5 scans from pointSel 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); - - if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } else { - if (pointSqDis < minPointSqDis3) { - minPointSqDis3 = pointSqDis; - minPointInd3 = j; - } + // squared distance between the other point and pointSel + 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); + + if (int(laserCloudSurfLast->points[j].intensity) <= + closestPointScan) { // within same scan + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } else { // not within same scan + if (pointSqDis < minPointSqDis3) { + minPointSqDis3 = pointSqDis; + minPointInd3 = j; + } } } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) { + for (int j = closestPointInd - 1; j >= 0; + j--) { // search backward points, find the other point in + // last cloud + if (int(laserCloudSurfLast->points[j].intensity) < + closestPointScan - 2.5) { // search another point within + // 2.5 scans from pointSel 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); - - if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) { + // squared distance between the other point and pointSel + 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); + + if (int(laserCloudSurfLast->points[j].intensity) >= + closestPointScan) { // within same scan if (pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } - } else { + } else { // not within same scan if (pointSqDis < minPointSqDis3) { minPointSqDis3 = pointSqDis; minPointInd3 = j; @@ -642,22 +820,24 @@ int main(int argc, char** argv) } } + // the planar patch pointSearchSurfInd1[i] = closestPointInd; pointSearchSurfInd2[i] = minPointInd2; pointSearchSurfInd3[i] = minPointInd3; } - if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { + if (pointSearchSurfInd2[i] >= 0 && + pointSearchSurfInd3[i] >= 0) { // found the planar patch 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 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); @@ -666,17 +846,30 @@ int main(int argc, char** argv) pc /= ps; pd /= ps; - float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; + // this is exactly the distance from pointSel to planar patch + // {tripod1, tripod2, tripod3} + float pd2 = + pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; - pointProj = pointSel; + // now + // pa == diff(pd2, x0) + // pb == diff(pd2, y0) + // pc == diff(pd2, z0) + + /*pointProj = pointSel; pointProj.x -= pa * pd2; pointProj.y -= pb * pd2; - pointProj.z -= pc * pd2; + pointProj.z -= pc * pd2;*/ - float s = 1; + float s = 1; // TODO: step? weight? if (iterCount >= 5) { - s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x - + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); + s = 1 - + 1.8 * fabs(pd2) / + sqrt( + sqrt(pointSel.x * + pointSel.x // TODO: why adjust s like this? + + pointSel.y * pointSel.y + + pointSel.z * pointSel.z)); } coeff.x = s * pa; @@ -684,7 +877,9 @@ int main(int argc, char** argv) coeff.z = s * pc; coeff.intensity = s * pd2; - if (s > 0.1 && pd2 != 0) { + if (s > 0.1 && pd2 != 0) { // apply this correspondence only when + // s is not too small and distance is + // not zero laserCloudOri->push_back(surfPointsFlat->points[i]); coeffSel->push_back(coeff); } @@ -703,12 +898,18 @@ int main(int argc, char** argv) cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); for (int i = 0; i < pointSelNum; i++) { - pointOri = laserCloudOri->points[i]; - coeff = coeffSel->points[i]; + pointOri = laserCloudOri->points[i]; // the original coordinates + // (not transformed to start + // time point) of feature point + // in current cloud + coeff = coeffSel->points[i]; // the scaled gradients: diff(distance, + // {x, y, z}), and the scaled distance. + // x/y/z are coordinates of feature + // points in the starting frame float s = 1; - float srx = sin(s * transform[0]); + float srx = sin(s * transform[0]); // float crx = cos(s * transform[0]); float sry = sin(s * transform[1]); float cry = cos(s * transform[1]); @@ -718,36 +919,61 @@ int main(int argc, char** argv) float ty = s * transform[4]; float tz = s * transform[5]; - 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 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; @@ -764,7 +990,7 @@ int main(int argc, char** argv) matAtB = matAt * matB; cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); - if (iterCount == 0) { + if (iterCount == 0) { // initialize cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0)); @@ -800,16 +1026,14 @@ int main(int argc, char** argv) transform[4] += matX.at(4, 0); transform[5] += matX.at(5, 0); - for(int i=0; i<6; i++){ - if(isnan(transform[i])) - transform[i]=0; + for (int i = 0; i < 6; i++) { + if (std::isnan(transform[i])) + transform[i] = 0; } - float deltaR = sqrt( - pow(rad2deg(matX.at(0, 0)), 2) + + float deltaR = sqrt(pow(rad2deg(matX.at(0, 0)), 2) + pow(rad2deg(matX.at(1, 0)), 2) + pow(rad2deg(matX.at(2, 0)), 2)); - float deltaT = sqrt( - pow(matX.at(3, 0) * 100, 2) + + float deltaT = sqrt(pow(matX.at(3, 0) * 100, 2) + pow(matX.at(4, 0) * 100, 2) + pow(matX.at(5, 0) * 100, 2)); @@ -819,14 +1043,16 @@ int main(int argc, char** argv) } } + // accumulate transform float rx, ry, rz, tx, ty, tz; - AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], - -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz); - - float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) - - sin(rz) * (transform[4] - imuShiftFromStartY); - float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) - + cos(rz) * (transform[4] - imuShiftFromStartY); + AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], + -transform[0], -transform[1] * 1.05, -transform[2], rx, + ry, rz); + + float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) - + sin(rz) * (transform[4] - imuShiftFromStartY); + float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) + + cos(rz) * (transform[4] - imuShiftFromStartY); float z1 = transform[5] * 1.05 - imuShiftFromStartZ; float x2 = x1; @@ -837,7 +1063,7 @@ int main(int argc, char** argv) ty = transformSum[4] - y2; tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2); - PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, + PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz); transformSum[0] = rx; @@ -847,7 +1073,8 @@ int main(int argc, char** argv) transformSum[4] = ty; transformSum[5] = tz; - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry); + geometry_msgs::Quaternion geoQuat = + tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry); laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserOdometry.pose.pose.orientation.x = -geoQuat.y; @@ -859,26 +1086,34 @@ int main(int argc, char** argv) laserOdometry.pose.pose.position.z = tz; pubLaserOdometry.publish(laserOdometry); + // laserOdometryFreq.report(); + laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat); - laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + laserOdometryTrans.setRotation( + tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz)); tfBroadcaster.sendTransform(laserOdometryTrans); int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); for (int i = 0; i < cornerPointsLessSharpNum; i++) { - TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[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]); + 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]); + TransformToEnd( + &laserCloudFullRes->points[i], + &laserCloudFullRes + ->points[i]); // transform all points in this sweep to end } } @@ -902,21 +1137,29 @@ int main(int argc, char** argv) sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); + laserCloudCornerLast2.header.stamp = + ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudCornerLast2.header.frame_id = "/camera"; - pubLaserCloudCornerLast.publish(laserCloudCornerLast2); + pubLaserCloudCornerLast.publish( + laserCloudCornerLast2); // all transformed to sweep end sensor_msgs::PointCloud2 laserCloudSurfLast2; pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); + laserCloudSurfLast2.header.stamp = + ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudSurfLast2.header.frame_id = "/camera"; - pubLaserCloudSurfLast.publish(laserCloudSurfLast2); + pubLaserCloudSurfLast.publish( + laserCloudSurfLast2); // all transformed to sweep end sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); + laserCloudFullRes3.header.stamp = + ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudFullRes3.header.frame_id = "/camera"; - pubLaserCloudFullRes.publish(laserCloudFullRes3); + pubLaserCloudFullRes.publish( + laserCloudFullRes3); // all transformed to sweep end + + // registeredLaserCloudFreq.report(); } } diff --git a/src/scanRegistration.cpp b/src/scanRegistration.cpp index f59f3cfe..b51eca5c 100755 --- a/src/scanRegistration.cpp +++ b/src/scanRegistration.cpp @@ -30,44 +30,67 @@ // 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 +#include #include #include +#include +#include +#include #include #include #include -#include #include +#include using std::sin; using std::cos; using std::atan2; -const double scanPeriod = 0.1; +#ifndef VELODYNE_HDL64E +const double scanPeriod = 0.1; // time duration per scan +#else +const double scanPeriod = 0.1; // TODO +#endif const int systemDelay = 20; int systemInitCount = 0; bool systemInited = false; -const int N_SCANS = 16; +#ifndef VELODYNE_HDL64E +const int N_SCANS = 16; ///// +#else +const int N_SCANS = 64; +#endif -float cloudCurvature[40000]; -int cloudSortInd[40000]; -int cloudNeighborPicked[40000]; -int cloudLabel[40000]; +#ifndef VELODYNE_HDL64E +const int MAX_POINTS = 40000; +#else +const int MAX_POINTS = 160000; +#endif + +float cloudCurvature[MAX_POINTS]; +int cloudSortInd[MAX_POINTS]; +int cloudNeighborPicked[MAX_POINTS]; +int cloudLabel[MAX_POINTS]; int imuPointerFront = 0; int imuPointerLast = -1; + +#ifndef VELODYNE_HDL64E const int imuQueLength = 200; +#else +const int imuQueLength = 2000; +#endif float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0; @@ -78,8 +101,10 @@ float imuShiftXStart = 0, imuShiftYStart = 0, imuShiftZStart = 0; float imuVeloXCur = 0, imuVeloYCur = 0, imuVeloZCur = 0; float imuShiftXCur = 0, imuShiftYCur = 0, imuShiftZCur = 0; -float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0; -float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0; +float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, + imuShiftFromStartZCur = 0; // updated by ShiftToStartIMU() +float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, + imuVeloFromStartZCur = 0; // updated by VeloToStartIMU() double imuTime[imuQueLength] = {0}; float imuRoll[imuQueLength] = {0}; @@ -89,11 +114,11 @@ float imuYaw[imuQueLength] = {0}; float imuAccX[imuQueLength] = {0}; float imuAccY[imuQueLength] = {0}; float imuAccZ[imuQueLength] = {0}; - +// updated by AccumulateIMUShift() float imuVeloX[imuQueLength] = {0}; float imuVeloY[imuQueLength] = {0}; float imuVeloZ[imuQueLength] = {0}; - +// updated by AccumulateIMUShift() float imuShiftX[imuQueLength] = {0}; float imuShiftY[imuQueLength] = {0}; float imuShiftZ[imuQueLength] = {0}; @@ -105,15 +130,21 @@ ros::Publisher pubSurfPointsFlat; ros::Publisher pubSurfPointsLessFlat; ros::Publisher pubImuTrans; -void ShiftToStartIMU(float pointTime) -{ - imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime; - imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime; - imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime; - - float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur; +// imu shift from start vector (imuShiftFromStart*Cur) converted into start imu +// coordinates? +void ShiftToStartIMU(float pointTime) { + imuShiftFromStartXCur = + imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime; + imuShiftFromStartYCur = + imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime; + imuShiftFromStartZCur = + imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime; + + float x1 = cos(imuYawStart) * imuShiftFromStartXCur - + sin(imuYawStart) * imuShiftFromStartZCur; float y1 = imuShiftFromStartYCur; - float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur; + float z1 = sin(imuYawStart) * imuShiftFromStartXCur + + cos(imuYawStart) * imuShiftFromStartZCur; float x2 = x1; float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; @@ -123,16 +154,18 @@ void ShiftToStartIMU(float pointTime) imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; imuShiftFromStartZCur = z2; } - -void VeloToStartIMU() -{ +// imu velocity from start vector (imuVeloFromStart*Cur) converted into start +// imu coordinates? +void VeloToStartIMU() { imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart; imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart; - float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur; + float x1 = cos(imuYawStart) * imuVeloFromStartXCur - + sin(imuYawStart) * imuVeloFromStartZCur; float y1 = imuVeloFromStartYCur; - float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur; + float z1 = sin(imuYawStart) * imuVeloFromStartXCur + + cos(imuYawStart) * imuVeloFromStartZCur; float x2 = x1; float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; @@ -142,9 +175,8 @@ void VeloToStartIMU() imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; imuVeloFromStartZCur = z2; } - -void TransformToStartIMU(PointType *p) -{ +// points converted into start imu coordinates? +void TransformToStartIMU(PointType *p) { float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y; float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y; float z1 = p->z; @@ -165,13 +197,15 @@ void TransformToStartIMU(PointType *p) float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4; float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4; - p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur; - p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur; + p->x = + cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur; + p->y = + -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur; p->z = z5 + imuShiftFromStartZCur; } - -void AccumulateIMUShift() -{ +// compute last shift to imuShift*[imuPointerLast] and velo to +// imuVelo*[imuPointerLast] using previous shift/velo/acc +void AccumulateIMUShift() { float roll = imuRoll[imuPointerLast]; float pitch = imuPitch[imuPointerLast]; float yaw = imuYaw[imuPointerLast]; @@ -195,12 +229,15 @@ void AccumulateIMUShift() double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; if (timeDiff < scanPeriod) { - imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff - + accX * timeDiff * timeDiff / 2; - imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff - + accY * timeDiff * timeDiff / 2; - imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff - + accZ * timeDiff * timeDiff / 2; + imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + + imuVeloX[imuPointerBack] * timeDiff + + accX * timeDiff * timeDiff / 2; + imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + + imuVeloY[imuPointerBack] * timeDiff + + accY * timeDiff * timeDiff / 2; + imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + + imuVeloZ[imuPointerBack] * timeDiff + + accZ * timeDiff * timeDiff / 2; imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff; imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff; @@ -208,8 +245,9 @@ void AccumulateIMUShift() } } -void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) -{ +auto last_time = std::chrono::system_clock::now(); + +void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) { if (!systemInited) { systemInitCount++; if (systemInitCount >= systemDelay) { @@ -218,48 +256,117 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) return; } - std::vector scanStartInd(N_SCANS, 0); - std::vector scanEndInd(N_SCANS, 0); - - double timeScanCur = laserCloudMsg->header.stamp.toSec(); - pcl::PointCloud laserCloudIn; + std::vector scanStartInd( + N_SCANS, 0); // scanStartInd[scanId] is the first point id of scanId + std::vector scanEndInd( + N_SCANS, 0); // scanEndInd[scanId] is the last point id of scanId + + double timeScanCur = + laserCloudMsg->header.stamp.toSec(); // time point of current scan + pcl::PointCloud + laserCloudIn; // input cloud, NaN points removed 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; + + //ROS_INFO("cloud recieved"); + if (false) { + // write clound to file + static bool written = false; + if (!written) { + std::ofstream ofs("/home/i-yanghao/tmp/normalized_cloud.xyz"); + if (ofs) { + for (int i = 0; i < laserCloudIn.points.size(); i++) { + auto & p = laserCloudIn.points[i]; + float len = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + ofs << p.x / len << " " << p.y / len << " " << p.z / len << std::endl; + } + ROS_INFO("cloud written"); + written = true; + } + } + } + + int cloudSize = laserCloudIn.points.size(); // number of cloud points + float startOri = + -atan2(laserCloudIn.points[0].y, + laserCloudIn.points[0] + .x); // ori of first point in cloud on origin x-y plane + float endOri = + -atan2(laserCloudIn.points[cloudSize - 1] + .y, // ori of last point in clound on origin x-y plane + 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); + std::vector> laserCloudScans(N_SCANS); + + // float minAngle = 180, maxAngle = -180; + // PointType minP, maxP; + // minP.x = minP.y = minP.z = 1e8; + // maxP.x = maxP.y = maxP.z = -1e8; + + /// use imu data to register original scanned points into lidar coodinates in + /// different scan lines 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; + // minP.x = std::min(minP.x, point.x); + // minP.y = std::min(minP.y, point.y); + // minP.z = std::min(minP.z, point.z); + // maxP.x = std::max(maxP.x, point.x); + // maxP.y = std::max(maxP.y, point.y); + // maxP.z = std::max(maxP.z, point.z); + + float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * + 180 / M_PI; // angle of origin z from origin x-y plane int scanID; - int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); - if (roundedAngle > 0){ + // if(!std::isnan(angle)) { + // minAngle = std::min(angle, minAngle); + // maxAngle = std::max(angle, maxAngle); + // } + // ROS_INFO("[%f]", angle); + + // compute scanID +#ifndef VELODYNE_HDL64E + int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : +0.5)); + if (roundedAngle > 0) { scanID = roundedAngle; - } - else { + } else { scanID = roundedAngle + (N_SCANS - 1); } - if (scanID > (N_SCANS - 1) || scanID < 0 ){ +#else + const float angleLowerBoundDeg = -24.8f; + const float angleUpperBoundDeg = 2.0f; + const float angleSpan = angleUpperBoundDeg - angleLowerBoundDeg; + const float angleStep = angleSpan / (N_SCANS - 1); + float angleID = (angle - angleLowerBoundDeg) / angleStep; + + scanID = int(angleID + 0.5f); +#endif + + if (scanID > (N_SCANS - 1) || scanID < 0) { // drop the points with invalid scanIDs count--; continue; } + const int debug_errorPointIDStart = 121513; + // if (i >= debug_errorPointIDStart) { + // ROS_INFO("point %i's scanID = %i", i, scanID); + // } + float ori = -atan2(point.x, point.z); + if (!halfPassed) { if (ori < startOri - M_PI / 2) { ori += 2 * M_PI; @@ -277,12 +384,17 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 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 (i >= debug_errorPointIDStart) { + // ROS_INFO("halfPassed = %i, ori = %f, point intensity = %f", halfPassed, + // ori, point.intensity); + // } + if (imuPointerLast >= 0) { float pointTime = relTime * scanPeriod; while (imuPointerFront != imuPointerLast) { @@ -292,7 +404,9 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) imuPointerFront = (imuPointerFront + 1) % imuQueLength; } - if (timeScanCur + pointTime > imuTime[imuPointerFront]) { + if (timeScanCur + pointTime > + imuTime[imuPointerFront]) { /// use the newest imu data if no newer + /// imu imuRollCur = imuRoll[imuPointerFront]; imuPitchCur = imuPitch[imuPointerFront]; imuYawCur = imuYaw[imuPointerFront]; @@ -304,30 +418,43 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) imuShiftXCur = imuShiftX[imuPointerFront]; imuShiftYCur = imuShiftY[imuPointerFront]; imuShiftZCur = imuShiftZ[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; + } else { /// interpolate in all existing imu data if there are newer imu + /// data + 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; + 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; + imuYawCur = imuYaw[imuPointerFront] * ratioFront + + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack; } else { - imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; + imuYawCur = imuYaw[imuPointerFront] * ratioFront + + imuYaw[imuPointerBack] * ratioBack; } - imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack; - imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack; - imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack; - - imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack; - imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack; - imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack; + imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + + imuVeloX[imuPointerBack] * ratioBack; + imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + + imuVeloY[imuPointerBack] * ratioBack; + imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + + imuVeloZ[imuPointerBack] * ratioBack; + + imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + + imuShiftX[imuPointerBack] * ratioBack; + imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + + imuShiftY[imuPointerBack] * ratioBack; + imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + + imuShiftZ[imuPointerBack] * ratioBack; } if (i == 0) { imuRollStart = imuRollCur; @@ -347,8 +474,20 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) TransformToStartIMU(&point); } } + laserCloudScans[scanID].push_back(point); } + + //ROS_INFO("all points are grouped"); + + // ROS_INFO("\n"); + // ROS_INFO("minAngle = %f, maxAngle = %f\n", minAngle, maxAngle); + // output minAngle = -15, maxAngle = 15 + // ROS_INFO("bounding box = [%f,%f,%f; %f,%f,%f]\n", minP.x, minP.y, minP.z, + // maxP.x, maxP.y, maxP.z); + // output generally: [-20(+-10), -5(+-1), -100(+-20); +70(+-10), +25(+-1), + // +80(+-10)] + cloudSize = count; pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); @@ -357,24 +496,25 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) } int scanCount = -1; for (int i = 5; i < cloudSize - 5; i++) { - float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x - + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x - + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x - + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x - + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x - + laserCloud->points[i + 5].x; - float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y - + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y - + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y - + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y - + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y - + laserCloud->points[i + 5].y; - float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z - + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z - + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z - + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z - + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z - + laserCloud->points[i + 5].z; + //ROS_INFO("i = %i, cloundSize = %i", i, cloudSize); + float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + + laserCloud->points[i + 5].x; + float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + + laserCloud->points[i + 5].y; + float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + + laserCloud->points[i + 5].z; cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; cloudSortInd[i] = i; cloudNeighborPicked[i] = 0; @@ -392,6 +532,8 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) scanStartInd[0] = 5; scanEndInd.back() = cloudSize - 5; + //ROS_INFO("cloudCurvature scanStartInd scanEndInd computed"); + for (int i = 5; i < cloudSize - 6; i++) { float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x; float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y; @@ -399,21 +541,25 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 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 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); + 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) { + 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) { // is connected? cloudNeighborPicked[i - 5] = 1; cloudNeighborPicked[i - 4] = 1; cloudNeighborPicked[i - 3] = 1; @@ -422,11 +568,15 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) cloudNeighborPicked[i] = 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) { + 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) { // is connected? cloudNeighborPicked[i + 1] = 1; cloudNeighborPicked[i + 2] = 1; cloudNeighborPicked[i + 3] = 1; @@ -442,30 +592,35 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 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; + 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; } } + //ROS_INFO("cloudNeighborPicked initialized"); - pcl::PointCloud cornerPointsSharp; - pcl::PointCloud cornerPointsLessSharp; - pcl::PointCloud surfPointsFlat; - pcl::PointCloud surfPointsLessFlat; + pcl::PointCloud cornerPointsSharp; // the outputs + pcl::PointCloud cornerPointsLessSharp; // the outputs + pcl::PointCloud surfPointsFlat; // the outputs + pcl::PointCloud surfPointsLessFlat; // the outputs for (int i = 0; i < N_SCANS; i++) { - pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); + pcl::PointCloud::Ptr surfPointsLessFlatScan( + new pcl::PointCloud); for (int j = 0; j < 6; j++) { - int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6; - int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1; - for (int k = sp + 1; k <= ep; k++) { + int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6; + int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1; + + for (int k = sp + 1; k <= ep; k++) { // sort by curvature within [sp, + // ep]?, curvature descending order for (int l = k; l >= sp + 1; l--) { - if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) { + if (cloudCurvature[cloudSortInd[l]] < + cloudCurvature[cloudSortInd[l - 1]]) { int temp = cloudSortInd[l - 1]; cloudSortInd[l - 1] = cloudSortInd[l]; cloudSortInd[l] = temp; @@ -476,9 +631,8 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) int largestPickedNum = 0; for (int k = ep; k >= sp; k--) { int ind = cloudSortInd[k]; - if (cloudNeighborPicked[ind] == 0 && - cloudCurvature[ind] > 0.1) { - + if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1) { + largestPickedNum++; if (largestPickedNum <= 2) { cloudLabel[ind] = 2; @@ -493,12 +647,12 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; 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; + 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; } @@ -506,12 +660,12 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; 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; + 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; } @@ -524,8 +678,7 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) int smallestPickedNum = 0; for (int k = sp; k <= ep; k++) { int ind = cloudSortInd[k]; - if (cloudNeighborPicked[ind] == 0 && - cloudCurvature[ind] < 0.1) { + if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) { cloudLabel[ind] = -1; surfPointsFlat.push_back(laserCloud->points[ind]); @@ -537,12 +690,12 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) cloudNeighborPicked[ind] = 1; for (int l = 1; l <= 5; 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; + 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; } @@ -550,12 +703,12 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) cloudNeighborPicked[ind + l] = 1; } for (int l = -1; l >= -5; 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; + 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; } @@ -581,6 +734,8 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) surfPointsLessFlat += surfPointsLessFlatScanDS; } + //ROS_INFO("feature points collected"); + sensor_msgs::PointCloud2 laserCloudOutMsg; pcl::toROSMsg(*laserCloud, laserCloudOutMsg); laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; @@ -633,10 +788,19 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) imuTransMsg.header.stamp = laserCloudMsg->header.stamp; imuTransMsg.header.frame_id = "/camera"; pubImuTrans.publish(imuTransMsg); + + // #define PRINT(name) ROS_INFO("in scanRegistration "#name" = %f", name) + // PRINT(imuShiftFromStartXCur); + // PRINT(imuShiftFromStartYCur); + // PRINT(imuShiftFromStartZCur); + // PRINT(imuVeloFromStartXCur); + // PRINT(imuVeloFromStartYCur); + // PRINT(imuVeloFromStartZCur); + // #undef PRINT } -void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) -{ +void imuHandler(const sensor_msgs::Imu::ConstPtr &imuIn) { + //ROS_INFO("imu recieved!\n"); double roll, pitch, yaw; tf::Quaternion orientation; tf::quaternionMsgToTF(imuIn->orientation, orientation); @@ -646,6 +810,12 @@ void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81; + //#define PRINT(name) ROS_INFO(#name" = %f\n", name) + // PRINT(accX); + // PRINT(accY); + // PRINT(accZ); + //#undef PRINT + imuPointerLast = (imuPointerLast + 1) % imuQueLength; imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); @@ -659,35 +829,34 @@ void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) AccumulateIMUShift(); } -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "scanRegistration"); ros::NodeHandle nh; - ros::Subscriber subLaserCloud = nh.subscribe - ("/velodyne_points", 2, laserCloudHandler); + ros::Subscriber subLaserCloud = nh.subscribe( + "/velodyne_points", 2, laserCloudHandler); - ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); + ros::Subscriber subImu = + nh.subscribe("/imu/data", 50, imuHandler); - pubLaserCloud = nh.advertise - ("/velodyne_cloud_2", 2); + pubLaserCloud = + nh.advertise("/velodyne_cloud_2", 2); - pubCornerPointsSharp = nh.advertise - ("/laser_cloud_sharp", 2); + pubCornerPointsSharp = + nh.advertise("/laser_cloud_sharp", 2); - pubCornerPointsLessSharp = nh.advertise - ("/laser_cloud_less_sharp", 2); + pubCornerPointsLessSharp = + nh.advertise("/laser_cloud_less_sharp", 2); - pubSurfPointsFlat = nh.advertise - ("/laser_cloud_flat", 2); + pubSurfPointsFlat = + nh.advertise("/laser_cloud_flat", 2); - pubSurfPointsLessFlat = nh.advertise - ("/laser_cloud_less_flat", 2); + pubSurfPointsLessFlat = + nh.advertise("/laser_cloud_less_flat", 2); - pubImuTrans = nh.advertise ("/imu_trans", 5); + pubImuTrans = nh.advertise("/imu_trans", 5); ros::spin(); return 0; } - diff --git a/src/transformMaintenance.cpp b/src/transformMaintenance.cpp old mode 100755 new mode 100644 index 292ead13..5913b69d --- a/src/transformMaintenance.cpp +++ b/src/transformMaintenance.cpp @@ -35,16 +35,16 @@ #include #include #include -#include -#include -#include #include #include +#include +#include +#include #include #include #include -#include #include +#include float transformSum[6] = {0}; float transformIncre[6] = {0}; @@ -57,13 +57,12 @@ 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]); +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 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; @@ -94,61 +93,90 @@ void transformAssociateToMap() 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); + 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]; + 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[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); + transformMapped[5] = transformAftMapped[5] - (-sin(transformMapped[1]) * x2 + + cos(transformMapped[1]) * z2); } -void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) -{ +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); + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) + .getRPY(roll, pitch, yaw); transformSum[0] = -pitch; transformSum[1] = -yaw; @@ -160,8 +188,8 @@ void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) transformAssociateToMap(); - geoQuat = tf::createQuaternionMsgFromRollPitchYaw - (transformMapped[2], -transformMapped[0], -transformMapped[1]); + geoQuat = tf::createQuaternionMsgFromRollPitchYaw( + transformMapped[2], -transformMapped[0], -transformMapped[1]); laserOdometry2.header.stamp = laserOdometry->header.stamp; laserOdometry2.pose.pose.orientation.x = -geoQuat.y; @@ -174,16 +202,18 @@ void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 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])); + 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) -{ +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); + tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) + .getRPY(roll, pitch, yaw); transformAftMapped[0] = -pitch; transformAftMapped[1] = -yaw; @@ -202,18 +232,18 @@ void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; } -int main(int argc, char** argv) -{ +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 subLaserOdometry = nh.subscribe( + "/laser_odom_to_init", 5, laserOdometryHandler); - ros::Subscriber subOdomAftMapped = nh.subscribe - ("/aft_mapped_to_init", 5, odomAftMappedHandler); + ros::Subscriber subOdomAftMapped = nh.subscribe( + "/aft_mapped_to_init", 5, odomAftMappedHandler); - ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); + ros::Publisher pubLaserOdometry2 = + nh.advertise("/integrated_to_init", 5); pubLaserOdometry2Pointer = &pubLaserOdometry2; laserOdometry2.header.frame_id = "/camera_init"; laserOdometry2.child_frame_id = "/camera"; diff --git a/tests/bag_test b/tests/bag_test old mode 100755 new mode 100644 diff --git a/vs/loam_velodyne.sln b/vs/loam_velodyne.sln new file mode 100644 index 00000000..97d7147b --- /dev/null +++ b/vs/loam_velodyne.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 14 +VisualStudioVersion = 14.0.25420.1 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "loam_velodyne", "loam_velodyne.vcxproj", "{966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Debug|x64.ActiveCfg = Debug|x64 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Debug|x64.Build.0 = Debug|x64 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Debug|x86.ActiveCfg = Debug|Win32 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Debug|x86.Build.0 = Debug|Win32 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Release|x64.ActiveCfg = Release|x64 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Release|x64.Build.0 = Release|x64 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Release|x86.ActiveCfg = Release|Win32 + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/vs/loam_velodyne.vcxproj b/vs/loam_velodyne.vcxproj new file mode 100644 index 00000000..2895ba0b --- /dev/null +++ b/vs/loam_velodyne.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + {966B3FE4-CAE4-473E-A5B1-6CD0E349A97A} + Win32Proj + loam_velodyne + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + + + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + C:\Tools\PCL 1.8.0\include\pcl-1.8;C:\Tools\PCL 1.8.0\3rdParty\Eigen\eigen3;C:\Tools\PCL 1.8.0\3rdParty\FLANN\include;C:\Tools\PCL 1.8.0\3rdParty\Boost\include\boost-1_61;C:\Tools\PCL 1.8.0\3rdParty\Qhull\include;C:\Tools\PCL 1.8.0\3rdParty\VTK\include;C:\Tools\OpenCV\2.4.13\include;%(AdditionalIncludeDirectories) + + + Console + true + + + + + + + Level3 + Disabled + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + C:\opt\ros\hydro\x86\include;C:\opt\rosdeps\hydro\x86\include;C:\opt\rosdeps\hydro\x86\include\eigen3;C:\Tools\OpenCV\2.4.13\include;C:\Tools\PCL 1.8.0\include\pcl-1.8;E:\loam_velodyne\include + + + Console + true + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + C:\Tools\PCL 1.8.0\include\pcl-1.8;C:\Tools\PCL 1.8.0\3rdParty\Eigen\eigen3;C:\Tools\PCL 1.8.0\3rdParty\FLANN\include;C:\Tools\PCL 1.8.0\3rdParty\Boost\include\boost-1_61;C:\Tools\PCL 1.8.0\3rdParty\Qhull\include;C:\Tools\PCL 1.8.0\3rdParty\VTK\include;C:\Tools\OpenCV\2.4.13\include;%(AdditionalIncludeDirectories) + + + Console + true + true + true + + + + + Level3 + + + MaxSpeed + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + C:\opt\ros\hydro\x86\include;C:\opt\rosdeps\hydro\x86\include;C:\opt\rosdeps\hydro\x86\include\eigen3;C:\Tools\OpenCV\2.4.13\include;C:\Tools\PCL 1.8.0\include\pcl-1.8;E:\loam_velodyne\include + + + Console + true + true + true + + + + + + \ No newline at end of file diff --git a/vs/loam_velodyne.vcxproj.filters b/vs/loam_velodyne.vcxproj.filters new file mode 100644 index 00000000..20d9137b --- /dev/null +++ b/vs/loam_velodyne.vcxproj.filters @@ -0,0 +1,31 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + \ No newline at end of file