diff --git a/CMakeLists.txt b/CMakeLists.txt index ce5e68c..c65a51b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,8 +34,8 @@ add_subdirectory(src/lib) add_executable(scanRegistration src/velodyne_scan_registration_node.cpp) target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) -add_executable(laserOdometry src/laserOdometry.cpp) -target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) +add_executable(laserOdometry src/laser_odometry_node.cpp) +target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) add_executable(laserMapping src/laserMapping.cpp) target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp deleted file mode 100755 index e1282a2..0000000 --- a/src/laserOdometry.cpp +++ /dev/null @@ -1,856 +0,0 @@ -// Copyright 2013, Ji Zhang, Carnegie Mellon University -// Further contributions copyright (c) 2016, Southwest Research Institute -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from this -// software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This is an implementation of the algorithm described in the following paper: -// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. -// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. - - -#include "loam_velodyne/nanoflann_pcl.h" -#include "lib/math_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace loam; - -const float scanPeriod = 0.1; - -const int skipFrameNum = 1; -bool systemInited = false; - -double timeCornerPointsSharp = 0; -double timeCornerPointsLessSharp = 0; -double timeSurfPointsFlat = 0; -double timeSurfPointsLessFlat = 0; -double timeLaserCloudFullRes = 0; -double timeImuTrans = 0; - -bool newCornerPointsSharp = false; -bool newCornerPointsLessSharp = false; -bool newSurfPointsFlat = false; -bool newSurfPointsLessFlat = false; -bool newLaserCloudFullRes = false; -bool newImuTrans = false; - -pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); -pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); -pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); -pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudOri(new pcl::PointCloud()); -pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); -pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); -pcl::PointCloud::Ptr imuTrans(new pcl::PointCloud()); - -nanoflann::KdTreeFLANN kdtreeCornerLast; -nanoflann::KdTreeFLANN kdtreeSurfLast; - -int laserCloudCornerLastNum; -int laserCloudSurfLastNum; - -int pointSelCornerInd[40000]; -float pointSearchCornerInd1[40000]; -float pointSearchCornerInd2[40000]; - -int pointSelSurfInd[40000]; -float pointSearchSurfInd1[40000]; -float pointSearchSurfInd2[40000]; -float pointSearchSurfInd3[40000]; - -Twist transform; -Twist transformSum; - -Angle imuRollStart, imuPitchStart, imuYawStart; -Angle imuRollLast, imuPitchLast, imuYawLast; - -Vector3 imuShiftFromStart; -Vector3 imuVeloFromStart; - -void TransformToStart(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); - - po->x = pi->x - s * transform.pos.x(); - po->y = pi->y - s * transform.pos.y(); - po->z = pi->z - s * transform.pos.z(); - po->intensity = pi->intensity; - - Angle rx = s * transform.rot_x.rad(); - Angle ry = s * transform.rot_y.rad(); - Angle rz = s * transform.rot_z.rad(); - rotateZXY(*po, -rz, -rx, -ry); -} - -void TransformToEnd(PointType const * const pi, PointType * const po) -{ - float s = 10 * (pi->intensity - int(pi->intensity)); - - po->x = pi->x - s * transform.pos.x(); - po->y = pi->y - s * transform.pos.y(); - po->z = pi->z - s * transform.pos.z(); - po->intensity = int(pi->intensity); - - Angle rx = s * transform.rot_x.rad(); - Angle ry = s * transform.rot_y.rad(); - Angle rz = s * transform.rot_z.rad(); - rotateZXY(*po, -rz, -rx, -ry); - rotateYXZ(*po, transform.rot_y, transform.rot_x, transform.rot_z); - - po->x += transform.pos.x() - imuShiftFromStart.x(); - po->y += transform.pos.y() - imuShiftFromStart.y(); - po->z += transform.pos.z() - imuShiftFromStart.z(); - - rotateZXY(*po, imuRollStart, imuPitchStart, imuYawStart); - rotateYXZ(*po, -imuYawLast, -imuPitchLast, -imuRollLast); -} - -void PluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, - const Angle& blx, const Angle& bly, const Angle& blz, - const Angle& alx, const Angle& aly, const Angle& alz, - Angle &acx, Angle &acy, Angle &acz) -{ - float sbcx = bcx.sin(); - float cbcx = bcx.cos(); - float sbcy = bcy.sin(); - float cbcy = bcy.cos(); - float sbcz = bcz.sin(); - float cbcz = bcz.cos(); - - float sblx = blx.sin(); - float cblx = blx.cos(); - float sbly = bly.sin(); - float cbly = bly.cos(); - float sblz = blz.sin(); - float cblz = blz.cos(); - - float salx = alx.sin(); - float calx = alx.cos(); - float saly = aly.sin(); - float caly = aly.cos(); - float salz = alz.sin(); - float calz = alz.cos(); - - float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); - acx = -asin(srx); - - float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); - float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); - acy = atan2(srycrx / acx.cos(), crycrx / acx.cos()); - - float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) - + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz - + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) - + calx*cblx*salz*sblz); - float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) - + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) - + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) - + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - - calx*calz*cblx*sblz); - acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos()); -} - -void AccumulateRotation(Angle cx, Angle cy, Angle cz, - Angle lx, Angle ly, Angle lz, - Angle &ox, Angle &oy, Angle &oz) -{ - float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin() - cx.cos()*cz.cos()*lx.sin() - lx.cos()*ly.cos()*cx.sin(); - ox = -asin(srx); - - float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin()) + lx.cos()*ly.sin()*(cy.cos()*cz.cos() - + cx.sin()*cy.sin()*cz.sin()) + lx.cos()*ly.cos()*cx.cos()*cy.sin(); - float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos() - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - - cy.cos()*cx.sin()*cz.sin()) - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin()); - oy = atan2(srycrx / ox.cos(), crycrx / ox.cos()); - - float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin()) + cx.cos()*cz.sin()*(ly.cos()*lz.cos() - + lx.sin()*ly.sin()*lz.sin()) + lx.cos()*cx.cos()*cz.cos()*lz.sin(); - float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos() - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - - lz.cos()*lx.sin()*ly.sin()) - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin()); - oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos()); -} - -void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2) -{ - timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec(); - - cornerPointsSharp->clear(); - pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp); - std::vector indices; - pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); - newCornerPointsSharp = true; -} - -void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2) -{ - timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec(); - - cornerPointsLessSharp->clear(); - pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp); - std::vector indices; - pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices); - newCornerPointsLessSharp = true; -} - -void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2) -{ - timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec(); - - surfPointsFlat->clear(); - pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat); - std::vector indices; - pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices); - newSurfPointsFlat = true; -} - -void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2) -{ - timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec(); - - surfPointsLessFlat->clear(); - pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat); - std::vector indices; - pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices); - newSurfPointsLessFlat = true; -} - -void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) -{ - timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); - - laserCloudFullRes->clear(); - pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); - std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices); - newLaserCloudFullRes = true; -} - -void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) -{ - timeImuTrans = imuTrans2->header.stamp.toSec(); - - imuTrans->clear(); - pcl::fromROSMsg(*imuTrans2, *imuTrans); - - imuPitchStart = imuTrans->points[0].x; - imuYawStart = imuTrans->points[0].y; - imuRollStart = imuTrans->points[0].z; - - imuPitchLast = imuTrans->points[1].x; - imuYawLast = imuTrans->points[1].y; - imuRollLast = imuTrans->points[1].z; - - imuShiftFromStart = imuTrans->points[2]; - imuVeloFromStart = imuTrans->points[3]; - - newImuTrans = true; -} - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "laserOdometry"); - ros::NodeHandle nh; - - ros::Subscriber subCornerPointsSharp = nh.subscribe - ("/laser_cloud_sharp", 2, laserCloudSharpHandler); - - ros::Subscriber subCornerPointsLessSharp = nh.subscribe - ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler); - - ros::Subscriber subSurfPointsFlat = nh.subscribe - ("/laser_cloud_flat", 2, laserCloudFlatHandler); - - ros::Subscriber subSurfPointsLessFlat = nh.subscribe - ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler); - - ros::Subscriber subLaserCloudFullRes = nh.subscribe - ("/velodyne_cloud_2", 2, laserCloudFullResHandler); - - ros::Subscriber subImuTrans = nh.subscribe - ("/imu_trans", 5, imuTransHandler); - - ros::Publisher pubLaserCloudCornerLast = nh.advertise - ("/laser_cloud_corner_last", 2); - - ros::Publisher pubLaserCloudSurfLast = nh.advertise - ("/laser_cloud_surf_last", 2); - - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/velodyne_cloud_3", 2); - - ros::Publisher pubLaserOdometry = nh.advertise ("/laser_odom_to_init", 5); - nav_msgs::Odometry laserOdometry; - laserOdometry.header.frame_id = "/camera_init"; - laserOdometry.child_frame_id = "/laser_odom"; - - tf::TransformBroadcaster tfBroadcaster; - tf::StampedTransform laserOdometryTrans; - laserOdometryTrans.frame_id_ = "/camera_init"; - laserOdometryTrans.child_frame_id_ = "/laser_odom"; - - PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff; - - bool isDegenerate = false; - Eigen::Matrix matP; - - int frameCount = skipFrameNum; - ros::Rate rate(100); - bool status = ros::ok(); - while (status) { - ros::spinOnce(); - - if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && - newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans && - fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 && - fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 && - fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 && - fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 && - fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) { - newCornerPointsSharp = false; - newCornerPointsLessSharp = false; - newSurfPointsFlat = false; - newSurfPointsLessFlat = false; - newLaserCloudFullRes = false; - newImuTrans = false; - - if (!systemInited) { - std::swap(cornerPointsLessSharp, laserCloudCornerLast); - std::swap(surfPointsLessFlat, laserCloudSurfLast); - - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudCornerLast2.header.frame_id = "/camera"; - pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudSurfLast2.header.frame_id = "/camera"; - pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - transformSum.rot_x += imuPitchStart; - transformSum.rot_z += imuRollStart; - - kdtreeCornerLast.setInputCloud(laserCloudCornerLast); - kdtreeSurfLast.setInputCloud(laserCloudSurfLast); - - systemInited = true; - continue; - } - - transform.pos -= imuVeloFromStart * scanPeriod; - - if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { - - std::vector pointSearchInd(1); - std::vector pointSearchSqDis(1); - std::vector indices; - - pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); - int cornerPointsSharpNum = cornerPointsSharp->points.size(); - int surfPointsFlatNum = surfPointsFlat->points.size(); - - for (int iterCount = 0; iterCount < 25; iterCount++) { - laserCloudOri->clear(); - coeffSel->clear(); - - for (int i = 0; i < cornerPointsSharpNum; i++) { - TransformToStart(&cornerPointsSharp->points[i], &pointSel); - - if (iterCount % 5 == 0) { - std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices); - kdtreeCornerLast.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); - - int closestPointInd = -1, minPointInd2 = -1; - if (pointSearchSqDis[0] < 25) { - closestPointInd = pointSearchInd[0]; - int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity); - - float pointSqDis, minPointSqDis2 = 25; - for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { - if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); - - if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } - } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudCornerLast->points[j], pointSel); - - if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } - } - } - - pointSearchCornerInd1[i] = closestPointInd; - pointSearchCornerInd2[i] = minPointInd2; - } - - if (pointSearchCornerInd2[i] >= 0) { - tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]]; - tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]]; - - float x0 = pointSel.x; - float y0 = pointSel.y; - float z0 = pointSel.z; - float x1 = tripod1.x; - float y1 = tripod1.y; - float z1 = tripod1.z; - float x2 = tripod2.x; - float y2 = tripod2.y; - float z2 = tripod2.z; - - float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) - * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); - - float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); - - float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; - - float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) - + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; - - float ld2 = a012 / l12; - - pointProj = pointSel; - pointProj.x -= la * ld2; - pointProj.y -= lb * ld2; - pointProj.z -= lc * ld2; - - float s = 1; - if (iterCount >= 5) { - s = 1 - 1.8 * fabs(ld2); - } - - coeff.x = s * la; - coeff.y = s * lb; - coeff.z = s * lc; - coeff.intensity = s * ld2; - - if (s > 0.1 && ld2 != 0) { - laserCloudOri->push_back(cornerPointsSharp->points[i]); - coeffSel->push_back(coeff); - } - } - } - - for (int i = 0; i < surfPointsFlatNum; i++) { - TransformToStart(&surfPointsFlat->points[i], &pointSel); - - if (iterCount % 5 == 0) { - kdtreeSurfLast.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); - int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; - if (pointSearchSqDis[0] < 25) { - closestPointInd = pointSearchInd[0]; - int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity); - - float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; - for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { - if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); - - if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } else { - if (pointSqDis < minPointSqDis3) { - minPointSqDis3 = pointSqDis; - minPointInd3 = j; - } - } - } - for (int j = closestPointInd - 1; j >= 0; j--) { - if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) { - break; - } - - pointSqDis = calcSquaredDiff(laserCloudSurfLast->points[j], pointSel); - - if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) { - if (pointSqDis < minPointSqDis2) { - minPointSqDis2 = pointSqDis; - minPointInd2 = j; - } - } else { - if (pointSqDis < minPointSqDis3) { - minPointSqDis3 = pointSqDis; - minPointInd3 = j; - } - } - } - } - - pointSearchSurfInd1[i] = closestPointInd; - pointSearchSurfInd2[i] = minPointInd2; - pointSearchSurfInd3[i] = minPointInd3; - } - - if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { - tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; - tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; - tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; - - float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); - float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); - float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); - float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); - - float ps = sqrt(pa * pa + pb * pb + pc * pc); - pa /= ps; - pb /= ps; - pc /= ps; - pd /= ps; - - float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; - - pointProj = pointSel; - pointProj.x -= pa * pd2; - pointProj.y -= pb * pd2; - pointProj.z -= pc * pd2; - - float s = 1; - if (iterCount >= 5) { - s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x - + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); - } - - coeff.x = s * pa; - coeff.y = s * pb; - coeff.z = s * pc; - coeff.intensity = s * pd2; - - if (s > 0.1 && pd2 != 0) { - laserCloudOri->push_back(surfPointsFlat->points[i]); - coeffSel->push_back(coeff); - } - } - } - - int pointSelNum = laserCloudOri->points.size(); - if (pointSelNum < 10) { - continue; - } - - Eigen::Matrix matA(pointSelNum, 6); - Eigen::Matrix matAt(6,pointSelNum); - Eigen::Matrix matAtA; - Eigen::VectorXf matB(pointSelNum); - Eigen::Matrix matAtB; - Eigen::Matrix matX; - - for (int i = 0; i < pointSelNum; i++) { - pointOri = laserCloudOri->points[i]; - coeff = coeffSel->points[i]; - - float s = 1; - - float srx = sin(s * transform.rot_x.rad()); - float crx = cos(s * transform.rot_x.rad()); - float sry = sin(s * transform.rot_y.rad()); - float cry = cos(s * transform.rot_y.rad()); - float srz = sin(s * transform.rot_z.rad()); - float crz = cos(s * transform.rot_z.rad()); - float tx = s * transform.pos.x(); - float ty = s * transform.pos.y(); - float tz = s * transform.pos.z(); - - float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z - + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x - + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z - + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y - + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z - + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z; - - float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x - + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z - + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) - + s*tz*crx*cry) * coeff.x - + ((s*cry*crz - s*srx*sry*srz)*pointOri.x - + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z - + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) - - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z; - - float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y - + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x - + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y - + s*ty*crx*srz + s*tx*crx*crz) * coeff.y - + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y - + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z; - - float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y - - s*(crz*sry + cry*srx*srz) * coeff.z; - - float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y - - s*(sry*srz - cry*crz*srx) * coeff.z; - - float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z; - - float d2 = coeff.intensity; - - matA(i, 0) = arx; - matA(i, 1) = ary; - matA(i, 2) = arz; - matA(i, 3) = atx; - matA(i, 4) = aty; - matA(i, 5) = atz; - matB(i, 0) = -0.05 * d2; - } - matAt = matA.transpose(); - matAtA = matAt * matA; - matAtB = matAt * matB; - - matX = matAtA.colPivHouseholderQr().solve(matAtB); - - if (iterCount == 0) { - Eigen::Matrix matE; - Eigen::Matrix matV; - Eigen::Matrix matV2; - - Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); - matE = esolver.eigenvalues().real(); - matV = esolver.eigenvectors().real(); - - matV2 = matV; - - isDegenerate = false; - float eignThre[6] = {10, 10, 10, 10, 10, 10}; - for (int i = 5; i >= 0; i--) { - if (matE(0, i) < eignThre[i]) { - for (int j = 0; j < 6; j++) { - matV2(i, j) = 0; - } - isDegenerate = true; - } else { - break; - } - } - matP = matV.inverse() * matV2; - } - - if (isDegenerate) { - Eigen::Matrix matX2; - matX2 = matX; - matX = matP * matX2; - } - - transform.rot_x = transform.rot_x.rad() + matX(0, 0); - transform.rot_y = transform.rot_y.rad() + matX(1, 0); - transform.rot_z = transform.rot_z.rad() + matX(2, 0); - transform.pos.x() += matX(3, 0); - transform.pos.y() += matX(4, 0); - transform.pos.z() += matX(5, 0); - - if( isnan(transform.rot_x.rad()) ) transform.rot_x = Angle(); - if( isnan(transform.rot_y.rad()) ) transform.rot_y = Angle(); - if( isnan(transform.rot_z.rad()) ) transform.rot_z = Angle(); - - if( isnan(transform.pos.x()) ) transform.pos.x() = 0.0; - if( isnan(transform.pos.y()) ) transform.pos.y() = 0.0; - if( isnan(transform.pos.z()) ) transform.pos.z() = 0.0; - - float deltaR = sqrt( - pow(rad2deg(matX(0, 0)), 2) + - pow(rad2deg(matX(1, 0)), 2) + - pow(rad2deg(matX(2, 0)), 2)); - float deltaT = sqrt( - pow(matX(3, 0) * 100, 2) + - pow(matX(4, 0) * 100, 2) + - pow(matX(5, 0) * 100, 2)); - - if (deltaR < 0.1 && deltaT < 0.1) { - break; - } - } - } - - Angle rx, ry, rz; - - AccumulateRotation(transformSum.rot_x, - transformSum.rot_y, - transformSum.rot_z, - -transform.rot_x, - -transform.rot_y.rad() * 1.05, - -transform.rot_z, - rx, ry, rz); - - Vector3 v( transform.pos.x() - imuShiftFromStart.x(), - transform.pos.y() - imuShiftFromStart.y(), - transform.pos.z()*1.05 - imuShiftFromStart.z() ); - rotateZXY(v, rz, rx, ry); - Vector3 trans = transformSum.pos - v; - - PluginIMURotation(rx, ry, rz, - imuPitchStart, imuYawStart, imuRollStart, - imuPitchLast, imuYawLast, imuRollLast, - rx, ry, rz); - - transformSum.rot_x = rx; - transformSum.rot_y = ry; - transformSum.rot_z = rz; - transformSum.pos = trans; - - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); - - laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserOdometry.pose.pose.orientation.x = -geoQuat.y; - laserOdometry.pose.pose.orientation.y = -geoQuat.z; - laserOdometry.pose.pose.orientation.z = geoQuat.x; - laserOdometry.pose.pose.orientation.w = geoQuat.w; - laserOdometry.pose.pose.position.x = trans.x(); - laserOdometry.pose.pose.position.y = trans.y(); - laserOdometry.pose.pose.position.z = trans.z(); - pubLaserOdometry.publish(laserOdometry); - - laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat); - laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); - laserOdometryTrans.setOrigin(tf::Vector3( trans.x(), trans.y(), trans.z()) ); - tfBroadcaster.sendTransform(laserOdometryTrans); - - int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); - for (int i = 0; i < cornerPointsLessSharpNum; i++) { - TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); - } - - int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); - for (int i = 0; i < surfPointsLessFlatNum; i++) { - TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); - } - - frameCount++; - if (frameCount >= skipFrameNum + 1) { - int laserCloudFullResNum = laserCloudFullRes->points.size(); - for (int i = 0; i < laserCloudFullResNum; i++) { - TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); - } - } - - std::swap(cornerPointsLessSharp, laserCloudCornerLast); - std::swap(surfPointsLessFlat, laserCloudSurfLast); - - laserCloudCornerLastNum = laserCloudCornerLast->points.size(); - laserCloudSurfLastNum = laserCloudSurfLast->points.size(); - - if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) - { - kdtreeCornerLast.setInputCloud(laserCloudCornerLast); - kdtreeSurfLast.setInputCloud(laserCloudSurfLast); - } - - if (frameCount >= skipFrameNum + 1) { - frameCount = 0; - - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudCornerLast2.header.frame_id = "/camera"; - pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudSurfLast2.header.frame_id = "/camera"; - pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); - laserCloudFullRes3.header.frame_id = "/camera"; - pubLaserCloudFullRes.publish(laserCloudFullRes3); - } - } - - status = ros::ok(); - rate.sleep(); - } - - return 0; -} diff --git a/src/laser_odometry_node.cpp b/src/laser_odometry_node.cpp new file mode 100644 index 0000000..b1187bd --- /dev/null +++ b/src/laser_odometry_node.cpp @@ -0,0 +1,20 @@ +#include +#include "lib/LaserOdometry.h" + + +/** Main node entry point. */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "laserOdometry"); + ros::NodeHandle node; + ros::NodeHandle privateNode("~"); + + loam::LaserOdometry laserOdom(0.1); + + if (laserOdom.setup(node, privateNode)) { + // initialization successful + laserOdom.spin(); + } + + return 0; +} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 5350aa7..744b869 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -4,5 +4,7 @@ add_library(loam ScanRegistration.h ScanRegistration.cpp VelodyneScanRegistration.h - VelodyneScanRegistration.cpp) + VelodyneScanRegistration.cpp + LaserOdometry.h + LaserOdometry.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp new file mode 100644 index 0000000..246ed9d --- /dev/null +++ b/src/lib/LaserOdometry.cpp @@ -0,0 +1,884 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#include "LaserOdometry.h" + +#include +#include +#include +#include + + +namespace loam { + +using std::sin; +using std::cos; +using std::asin; +using std::atan2; +using std::sqrt; +using std::fabs; +using std::pow; + + +LaserOdometry::LaserOdometry(const float& scanPeriod, + const uint16_t& ioRatio) + : _scanPeriod(scanPeriod), + _ioRatio(ioRatio), + _systemInited(false), + _frameCount(0), + _cornerPointsSharp(new pcl::PointCloud()), + _cornerPointsLessSharp(new pcl::PointCloud()), + _surfPointsFlat(new pcl::PointCloud()), + _surfPointsLessFlat(new pcl::PointCloud()), + _laserCloud(new pcl::PointCloud()), + _imuTrans(new pcl::PointCloud()), + _lastCornerCloud(new pcl::PointCloud()), + _lastSurfaceCloud(new pcl::PointCloud()), + _laserCloudOri(new pcl::PointCloud()), + _coeffSel(new pcl::PointCloud()) +{ + // initialize odometry and odometry tf messages + _laserOdometry.header.frame_id = "/camera_init"; + _laserOdometry.child_frame_id = "/laser_odom"; + + _laserOdometryTrans.frame_id_ = "/camera_init"; + _laserOdometryTrans.child_frame_id_ = "/laser_odom"; +} + + + +bool LaserOdometry::setup(ros::NodeHandle &node, + ros::NodeHandle &privateNode) +{ + // advertise laser odometry topics + _pubLaserCloudCornerLast = node.advertise("/laser_cloud_corner_last", 2); + _pubLaserCloudSurfLast = node.advertise("/laser_cloud_surf_last", 2); + _pubLaserCloudFullRes = node.advertise("/velodyne_cloud_3", 2); + _pubLaserOdometry = node.advertise("/laser_odom_to_init", 5); + + // subscribe to scan registration topics + _subCornerPointsSharp = node.subscribe + ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this); + + _subCornerPointsLessSharp = node.subscribe + ("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this); + + _subSurfPointsFlat = node.subscribe + ("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this); + + _subSurfPointsLessFlat = node.subscribe + ("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this); + + _subLaserCloudFullRes = node.subscribe + ("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this); + + _subImuTrans = node.subscribe + ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this); + + // set active mode + _activeMode = true; + + return true; +} + + + +void LaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po) +{ + float s = 10 * (pi.intensity - int(pi.intensity)); + + po.x = pi.x - s * _transform.pos.x(); + po.y = pi.y - s * _transform.pos.y(); + po.z = pi.z - s * _transform.pos.z(); + po.intensity = pi.intensity; + + Angle rx = s * _transform.rot_x.rad(); + Angle ry = s * _transform.rot_y.rad(); + Angle rz = s * _transform.rot_z.rad(); + rotateZXY(po, -rz, -rx, -ry); +} + + + +size_t LaserOdometry::transformToEnd(pcl::PointCloud::Ptr& cloud) +{ + size_t cloudSize = cloud->points.size(); + + for (size_t i = 0; i < cloudSize; i++) { + pcl::PointXYZI& point = cloud->points[i]; + + float s = 10 * (point.intensity - int(point.intensity)); + + point.x -= s * _transform.pos.x(); + point.y -= s * _transform.pos.y(); + point.z -= s * _transform.pos.z(); + point.intensity = int(point.intensity); + + Angle rx = s * _transform.rot_x.rad(); + Angle ry = s * _transform.rot_y.rad(); + Angle rz = s * _transform.rot_z.rad(); + rotateZXY(point, -rz, -rx, -ry); + rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z); + + point.x += _transform.pos.x() - _imuShiftFromStart.x(); + point.y += _transform.pos.y() - _imuShiftFromStart.y(); + point.z += _transform.pos.z() - _imuShiftFromStart.z(); + + rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart); + rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd); + } + + return cloudSize; +} + + + +void LaserOdometry::pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, + const Angle& blx, const Angle& bly, const Angle& blz, + const Angle& alx, const Angle& aly, const Angle& alz, + Angle &acx, Angle &acy, Angle &acz) +{ + float sbcx = bcx.sin(); + float cbcx = bcx.cos(); + float sbcy = bcy.sin(); + float cbcy = bcy.cos(); + float sbcz = bcz.sin(); + float cbcz = bcz.cos(); + + float sblx = blx.sin(); + float cblx = blx.cos(); + float sbly = bly.sin(); + float cbly = bly.cos(); + float sblz = blz.sin(); + float cblz = blz.cos(); + + float salx = alx.sin(); + float calx = alx.cos(); + float saly = aly.sin(); + float caly = aly.cos(); + float salz = alz.sin(); + float calz = alz.cos(); + + float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) + - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); + acx = -asin(srx); + + float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) + - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) + - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); + acy = atan2(srycrx / acx.cos(), crycrx / acx.cos()); + + float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) + - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) + - calx*cblx*cblz*salz) + + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + + calx*cblx*salz*sblz); + float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + + calx*calz*cblx*cblz) + - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) + - calx*calz*cblx*sblz); + acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos()); +} + + + +void LaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz, + Angle lx, Angle ly, Angle lz, + Angle &ox, Angle &oy, Angle &oz) +{ + float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin() + - cx.cos()*cz.cos()*lx.sin() + - lx.cos()*ly.cos()*cx.sin(); + ox = -asin(srx); + + float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin()) + + lx.cos()*ly.sin()*(cy.cos()*cz.cos() + cx.sin()*cy.sin()*cz.sin()) + + lx.cos()*ly.cos()*cx.cos()*cy.sin(); + float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos() + - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - cy.cos()*cx.sin()*cz.sin()) + - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin()); + oy = atan2(srycrx / ox.cos(), crycrx / ox.cos()); + + float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin()) + + cx.cos()*cz.sin()*(ly.cos()*lz.cos() + lx.sin()*ly.sin()*lz.sin()) + + lx.cos()*cx.cos()*cz.cos()*lz.sin(); + float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos() + - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - lz.cos()*lx.sin()*ly.sin()) + - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin()); + oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos()); +} + + + +void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg) +{ + _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp.toSec(); + + _cornerPointsSharp->clear(); + pcl::fromROSMsg(*cornerPointsSharpMsg, *_cornerPointsSharp); + std::vector indices; + pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices); + _newCornerPointsSharp = true; +} + + + +void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg) +{ + _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp.toSec(); + + _cornerPointsLessSharp->clear(); + pcl::fromROSMsg(*cornerPointsLessSharpMsg, *_cornerPointsLessSharp); + std::vector indices; + pcl::removeNaNFromPointCloud(*_cornerPointsLessSharp, *_cornerPointsLessSharp, indices); + _newCornerPointsLessSharp = true; +} + + + +void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg) +{ + _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp.toSec(); + + _surfPointsFlat->clear(); + pcl::fromROSMsg(*surfPointsFlatMsg, *_surfPointsFlat); + std::vector indices; + pcl::removeNaNFromPointCloud(*_surfPointsFlat, *_surfPointsFlat, indices); + _newSurfPointsFlat = true; +} + + + +void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg) +{ + _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp.toSec(); + + _surfPointsLessFlat->clear(); + pcl::fromROSMsg(*surfPointsLessFlatMsg, *_surfPointsLessFlat); + std::vector indices; + pcl::removeNaNFromPointCloud(*_surfPointsLessFlat, *_surfPointsLessFlat, indices); + _newSurfPointsLessFlat = true; +} + + + +void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) +{ + _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp.toSec(); + + _laserCloud->clear(); + pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloud); + std::vector indices; + pcl::removeNaNFromPointCloud(*_laserCloud, *_laserCloud, indices); + _newLaserCloudFullRes = true; +} + + + +void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg) +{ + _timeImuTrans = imuTransMsg->header.stamp.toSec(); + + _imuTrans->clear(); + pcl::fromROSMsg(*imuTransMsg, *_imuTrans); + + _imuPitchStart = _imuTrans->points[0].x; + _imuYawStart = _imuTrans->points[0].y; + _imuRollStart = _imuTrans->points[0].z; + + _imuPitchEnd = _imuTrans->points[1].x; + _imuYawEnd = _imuTrans->points[1].y; + _imuRollEnd = _imuTrans->points[1].z; + + _imuShiftFromStart = _imuTrans->points[2]; + _imuVeloFromStart = _imuTrans->points[3]; + + _newImuTrans = true; +} + + + +void LaserOdometry::spin() +{ + if (!_activeMode) { + return; + } + + ros::Rate rate(100); + bool status = ros::ok(); + + // loop until shutdown + while (status) { + ros::spinOnce(); + + // try processing new data + process(); + + status = ros::ok(); + rate.sleep(); + } +} + + + +void LaserOdometry::reset() +{ + _newCornerPointsSharp = false; + _newCornerPointsLessSharp = false; + _newSurfPointsFlat = false; + _newSurfPointsLessFlat = false; + _newLaserCloudFullRes = false; + _newImuTrans = false; +} + + + +bool LaserOdometry::hasNewData() +{ + return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat && + _newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans && + fabs(_timeCornerPointsSharp - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeCornerPointsLessSharp - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeSurfPointsFlat - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeLaserCloudFullRes - _timeSurfPointsLessFlat) < 0.005 && + fabs(_timeImuTrans - _timeSurfPointsLessFlat) < 0.005; +} + + + +void LaserOdometry::process() +{ + if (!hasNewData()) { + // waiting for new data to arrive... + return; + } + + // reset flags, etc. + reset(); + + if (!_systemInited) { + _cornerPointsLessSharp.swap(_lastCornerCloud); + _surfPointsLessFlat.swap(_lastSurfaceCloud); + + _lastCornerCloudSize = _lastCornerCloud->points.size(); + _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + + _lastCornerKDTree.setInputCloud(_lastCornerCloud); + _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); + + _transformSum.rot_x += _imuPitchStart; + _transformSum.rot_z += _imuRollStart; + + _systemInited = true; + return; + } + + pcl::PointXYZI coeff; + bool isDegenerate = false; + Eigen::Matrix matP; + + _frameCount++; + _transform.pos -= _imuVeloFromStart * _scanPeriod; + + if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + std::vector pointSearchInd(1); + std::vector pointSearchSqDis(1); + std::vector indices; + + pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices); + size_t cornerPointsSharpNum = _cornerPointsSharp->points.size(); + size_t surfPointsFlatNum = _surfPointsFlat->points.size(); + + _pointSearchCornerInd1.resize(cornerPointsSharpNum); + _pointSearchCornerInd2.resize(cornerPointsSharpNum); + _pointSearchSurfInd1.resize(surfPointsFlatNum); + _pointSearchSurfInd2.resize(surfPointsFlatNum); + _pointSearchSurfInd3.resize(surfPointsFlatNum); + + for (int iterCount = 0; iterCount < 25; iterCount++) { + pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3; + _laserCloudOri->clear(); + _coeffSel->clear(); + + for (int i = 0; i < cornerPointsSharpNum; i++) { + transformToStart(_cornerPointsSharp->points[i], pointSel); + + if (iterCount % 5 == 0) { + pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices); + _lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); + + int closestPointInd = -1, minPointInd2 = -1; + if (pointSearchSqDis[0] < 25) { + closestPointInd = pointSearchInd[0]; + int closestPointScan = int(_lastCornerCloud->points[closestPointInd].intensity); + + float pointSqDis, minPointSqDis2 = 25; + for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { + if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); + + if (int(_lastCornerCloud->points[j].intensity) > closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } + } + for (int j = closestPointInd - 1; j >= 0; j--) { + if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); + + if (int(_lastCornerCloud->points[j].intensity) < closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } + } + } + + _pointSearchCornerInd1[i] = closestPointInd; + _pointSearchCornerInd2[i] = minPointInd2; + } + + if (_pointSearchCornerInd2[i] >= 0) { + tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]]; + tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]]; + + float x0 = pointSel.x; + float y0 = pointSel.y; + float z0 = pointSel.z; + float x1 = tripod1.x; + float y1 = tripod1.y; + float z1 = tripod1.z; + float x2 = tripod2.x; + float y2 = tripod2.y; + float z2 = tripod2.z; + + float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) + * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); + + float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); + + float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; + + float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; + + float ld2 = a012 / l12; + + // TODO: Why writing to a variable that's never read? + pointProj = pointSel; + pointProj.x -= la * ld2; + pointProj.y -= lb * ld2; + pointProj.z -= lc * ld2; + + float s = 1; + if (iterCount >= 5) { + s = 1 - 1.8f * fabs(ld2); + } + + coeff.x = s * la; + coeff.y = s * lb; + coeff.z = s * lc; + coeff.intensity = s * ld2; + + if (s > 0.1 && ld2 != 0) { + _laserCloudOri->push_back(_cornerPointsSharp->points[i]); + _coeffSel->push_back(coeff); + } + } + } + + for (int i = 0; i < surfPointsFlatNum; i++) { + transformToStart(_surfPointsFlat->points[i], pointSel); + + if (iterCount % 5 == 0) { + _lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); + int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; + if (pointSearchSqDis[0] < 25) { + closestPointInd = pointSearchInd[0]; + int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity); + + float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; + for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { + if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); + + if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } else { + if (pointSqDis < minPointSqDis3) { + minPointSqDis3 = pointSqDis; + minPointInd3 = j; + } + } + } + for (int j = closestPointInd - 1; j >= 0; j--) { + if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5) { + break; + } + + pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); + + if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan) { + if (pointSqDis < minPointSqDis2) { + minPointSqDis2 = pointSqDis; + minPointInd2 = j; + } + } else { + if (pointSqDis < minPointSqDis3) { + minPointSqDis3 = pointSqDis; + minPointInd3 = j; + } + } + } + } + + _pointSearchSurfInd1[i] = closestPointInd; + _pointSearchSurfInd2[i] = minPointInd2; + _pointSearchSurfInd3[i] = minPointInd3; + } + + if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0) { + tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]]; + tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]]; + tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]]; + + float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) + - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); + float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) + - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); + float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) + - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); + float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); + + float ps = sqrt(pa * pa + pb * pb + pc * pc); + pa /= ps; + pb /= ps; + pc /= ps; + pd /= ps; + + float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; + + // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards? + pointProj = pointSel; + pointProj.x -= pa * pd2; + pointProj.y -= pb * pd2; + pointProj.z -= pc * pd2; + + float s = 1; + if (iterCount >= 5) { + s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel)); + } + + coeff.x = s * pa; + coeff.y = s * pb; + coeff.z = s * pc; + coeff.intensity = s * pd2; + + if (s > 0.1 && pd2 != 0) { + _laserCloudOri->push_back(_surfPointsFlat->points[i]); + _coeffSel->push_back(coeff); + } + } + } + + int pointSelNum = _laserCloudOri->points.size(); + if (pointSelNum < 10) { + continue; + } + + Eigen::Matrix matA(pointSelNum, 6); + Eigen::Matrix matAt(6,pointSelNum); + Eigen::Matrix matAtA; + Eigen::VectorXf matB(pointSelNum); + Eigen::Matrix matAtB; + Eigen::Matrix matX; + + for (int i = 0; i < pointSelNum; i++) { + const pcl::PointXYZI& pointOri = _laserCloudOri->points[i]; + coeff = _coeffSel->points[i]; + + float s = 1; + + float srx = sin(s * _transform.rot_x.rad()); + float crx = cos(s * _transform.rot_x.rad()); + float sry = sin(s * _transform.rot_y.rad()); + float cry = cos(s * _transform.rot_y.rad()); + float srz = sin(s * _transform.rot_z.rad()); + float crz = cos(s * _transform.rot_z.rad()); + float tx = s * _transform.pos.x(); + float ty = s * _transform.pos.y(); + float tz = s * _transform.pos.z(); + + float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z + + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x + + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z + + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y + + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z + + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z; + + float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x + + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z + + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) + + s*tz*crx*cry) * coeff.x + + ((s*cry*crz - s*srx*sry*srz)*pointOri.x + + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z + + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) + - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z; + + float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y + + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x + + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y + + s*ty*crx*srz + s*tx*crx*crz) * coeff.y + + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y + + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z; + + float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y + - s*(crz*sry + cry*srx*srz) * coeff.z; + + float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y + - s*(sry*srz - cry*crz*srx) * coeff.z; + + float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z; + + float d2 = coeff.intensity; + + matA(i, 0) = arx; + matA(i, 1) = ary; + matA(i, 2) = arz; + matA(i, 3) = atx; + matA(i, 4) = aty; + matA(i, 5) = atz; + matB(i, 0) = -0.05 * d2; + } + matAt = matA.transpose(); + matAtA = matAt * matA; + matAtB = matAt * matB; + + matX = matAtA.colPivHouseholderQr().solve(matAtB); + + if (iterCount == 0) { + Eigen::Matrix matE; + Eigen::Matrix matV; + Eigen::Matrix matV2; + + Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); + matE = esolver.eigenvalues().real(); + matV = esolver.eigenvectors().real(); + + matV2 = matV; + + isDegenerate = false; + float eignThre[6] = {10, 10, 10, 10, 10, 10}; + for (int i = 5; i >= 0; i--) { + if (matE(0, i) < eignThre[i]) { + for (int j = 0; j < 6; j++) { + matV2(i, j) = 0; + } + isDegenerate = true; + } else { + break; + } + } + matP = matV.inverse() * matV2; + } + + if (isDegenerate) { + Eigen::Matrix matX2; + matX2 = matX; + matX = matP * matX2; + } + + _transform.rot_x = _transform.rot_x.rad() + matX(0, 0); + _transform.rot_y = _transform.rot_y.rad() + matX(1, 0); + _transform.rot_z = _transform.rot_z.rad() + matX(2, 0); + _transform.pos.x() += matX(3, 0); + _transform.pos.y() += matX(4, 0); + _transform.pos.z() += matX(5, 0); + + if( isnan(_transform.rot_x.rad()) ) _transform.rot_x = Angle(); + if( isnan(_transform.rot_y.rad()) ) _transform.rot_y = Angle(); + if( isnan(_transform.rot_z.rad()) ) _transform.rot_z = Angle(); + + if( isnan(_transform.pos.x()) ) _transform.pos.x() = 0.0; + if( isnan(_transform.pos.y()) ) _transform.pos.y() = 0.0; + if( isnan(_transform.pos.z()) ) _transform.pos.z() = 0.0; + + float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) + + pow(rad2deg(matX(1, 0)), 2) + + pow(rad2deg(matX(2, 0)), 2)); + float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + + pow(matX(4, 0) * 100, 2) + + pow(matX(5, 0) * 100, 2)); + + if (deltaR < 0.1 && deltaT < 0.1) { + break; + } + } + } + + Angle rx, ry, rz; + + accumulateRotation(_transformSum.rot_x, + _transformSum.rot_y, + _transformSum.rot_z, + -_transform.rot_x, + -_transform.rot_y.rad() * 1.05, + -_transform.rot_z, + rx, ry, rz); + + Vector3 v( _transform.pos.x() - _imuShiftFromStart.x(), + _transform.pos.y() - _imuShiftFromStart.y(), + _transform.pos.z() * 1.05 - _imuShiftFromStart.z() ); + rotateZXY(v, rz, rx, ry); + Vector3 trans = _transformSum.pos - v; + + pluginIMURotation(rx, ry, rz, + _imuPitchStart, _imuYawStart, _imuRollStart, + _imuPitchEnd, _imuYawEnd, _imuRollEnd, + rx, ry, rz); + + _transformSum.rot_x = rx; + _transformSum.rot_y = ry; + _transformSum.rot_z = rz; + _transformSum.pos = trans; + + geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz.rad(), -rx.rad(), -ry.rad()); + + _laserOdometry.header.stamp = ros::Time().fromSec(_timeSurfPointsLessFlat); + _laserOdometry.pose.pose.orientation.x = -geoQuat.y; + _laserOdometry.pose.pose.orientation.y = -geoQuat.z; + _laserOdometry.pose.pose.orientation.z = geoQuat.x; + _laserOdometry.pose.pose.orientation.w = geoQuat.w; + _laserOdometry.pose.pose.position.x = trans.x(); + _laserOdometry.pose.pose.position.y = trans.y(); + _laserOdometry.pose.pose.position.z = trans.z(); + if (_activeMode) { + _pubLaserOdometry.publish(_laserOdometry); + } + + _laserOdometryTrans.stamp_ = ros::Time().fromSec(_timeSurfPointsLessFlat); + _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); + _laserOdometryTrans.setOrigin(tf::Vector3( trans.x(), trans.y(), trans.z()) ); + _tfBroadcaster.sendTransform(_laserOdometryTrans); + + transformToEnd(_cornerPointsLessSharp); + transformToEnd(_surfPointsLessFlat); + + _cornerPointsLessSharp.swap(_lastCornerCloud); + _surfPointsLessFlat.swap(_lastSurfaceCloud); + + _lastCornerCloudSize = _lastCornerCloud->points.size(); + _lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); + + if (_lastCornerCloudSize > 10 && _lastSurfaceCloudSize > 100) { + _lastCornerKDTree.setInputCloud(_lastCornerCloud); + _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); + } + + publishResult(); +} + + + +void LaserOdometry::publishResult() +{ + // only publish messages in active mode + if (!_activeMode) { + // transform each full resolution cloud in passive mode as we don't know who will be using it + transformToEnd(_laserCloud); + return; + } + + // publish results according to the input output ratio + if (_ioRatio < 2 || _frameCount % _ioRatio == 1) { + ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); + + sensor_msgs::PointCloud2 laserCloudCornerLast2; + pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); + laserCloudCornerLast2.header.stamp = sweepTime; + laserCloudCornerLast2.header.frame_id = "/camera"; + _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); + + sensor_msgs::PointCloud2 laserCloudSurfLast2; + pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); + laserCloudSurfLast2.header.stamp = sweepTime; + laserCloudSurfLast2.header.frame_id = "/camera"; + _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); + + sensor_msgs::PointCloud2 laserCloudFullRes3; + transformToEnd(_laserCloud); // transform full resolution cloud before sending it + pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = sweepTime; + laserCloudFullRes3.header.frame_id = "/camera"; + _pubLaserCloudFullRes.publish(laserCloudFullRes3); + } +} + +} // end namespace loam diff --git a/src/lib/LaserOdometry.h b/src/lib/LaserOdometry.h new file mode 100644 index 0000000..8bca58f --- /dev/null +++ b/src/lib/LaserOdometry.h @@ -0,0 +1,217 @@ +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This is an implementation of the algorithm described in the following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +#ifndef LOAM_LASERODOMETRY_H +#define LOAM_LASERODOMETRY_H + +#include "math_utils.h" +#include "loam_velodyne/nanoflann_pcl.h" + +#include +#include +#include +#include +#include +#include +#include + + +namespace loam { + +/** \brief Implementation of the LOAM laser odometry component. + * + */ +class LaserOdometry { +public: + LaserOdometry(const float& scanPeriod, + const uint16_t& ioRatio = 2); + + /** \brief Setup component in active mode. + * + * @param node the ROS node handle + * @param privateNode the private ROS node handle + */ + virtual bool setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode); + + /** \brief Handler method for a new sharp corner cloud. + * + * @param cornerPointsSharpMsg the new sharp corner cloud message + */ + void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg); + + /** \brief Handler method for a new less sharp corner cloud. + * + * @param cornerPointsLessSharpMsg the new less sharp corner cloud message + */ + void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg); + + /** \brief Handler method for a new flat surface cloud. + * + * @param surfPointsFlatMsg the new flat surface cloud message + */ + void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg); + + /** \brief Handler method for a new less flat surface cloud. + * + * @param surfPointsLessFlatMsg the new less flat surface cloud message + */ + void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg); + + /** \brief Handler method for a new full resolution cloud. + * + * @param laserCloudFullResMsg the new full resolution cloud message + */ + void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg); + + /** \brief Handler method for a new IMU transformation information. + * + * @param laserCloudFullResMsg the new IMU transformation information message + */ + void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg); + + /** \brief Process incoming messages in a loop until shutdown (used in active mode). */ + void spin(); + + /** \brief Try to process buffered data. */ + void process(); + + +protected: + /** \brief Reset flags, etc. */ + void reset(); + + /** \brief Check if all required information for a new processing step is available. */ + bool hasNewData(); + + /** \brief Transform the given point to the start of the sweep. + * + * @param pi the point to transform + * @param po the point instance for storing the result + */ + void transformToStart(const pcl::PointXYZI& pi, + pcl::PointXYZI& po); + + /** \brief Transform the given point cloud to the end of the sweep. + * + * @param cloud the point cloud to transform + */ + size_t transformToEnd(pcl::PointCloud::Ptr& cloud); + + void pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, + const Angle& blx, const Angle& bly, const Angle& blz, + const Angle& alx, const Angle& aly, const Angle& alz, + Angle &acx, Angle &acy, Angle &acz); + + void accumulateRotation(Angle cx, Angle cy, Angle cz, + Angle lx, Angle ly, Angle lz, + Angle &ox, Angle &oy, Angle &oz); + + /** \brief Publish the current result via the respective topics. */ + void publishResult(); + +private: + const float _scanPeriod; ///< time per scan + const uint16_t _ioRatio; ///< ratio of input to output frames + bool _systemInited; ///< initialization flag + long _frameCount; ///< number of processed frames + + pcl::PointCloud::Ptr _cornerPointsSharp; ///< sharp corner points cloud + pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud + pcl::PointCloud::Ptr _surfPointsFlat; ///< flat surface points cloud + pcl::PointCloud::Ptr _surfPointsLessFlat; ///< less flat surface points cloud + pcl::PointCloud::Ptr _laserCloud; ///< full resolution cloud + pcl::PointCloud::Ptr _imuTrans; ///< IMU transformation information + + pcl::PointCloud::Ptr _lastCornerCloud; ///< last corner points cloud + pcl::PointCloud::Ptr _lastSurfaceCloud; ///< last surface points cloud + pcl::PointCloud::Ptr _laserCloudOri; ///< point selection + pcl::PointCloud::Ptr _coeffSel; ///< point selection coefficients + + nanoflann::KdTreeFLANN _lastCornerKDTree; ///< last corner cloud KD-tree + nanoflann::KdTreeFLANN _lastSurfaceKDTree; ///< last surface cloud KD-tree + + double _timeCornerPointsSharp; ///< time of current sharp corner cloud + double _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud + double _timeSurfPointsFlat; ///< time of current flat surface cloud + double _timeSurfPointsLessFlat; ///< time of current less flat surface cloud + double _timeLaserCloudFullRes; ///< time of current full resolution cloud + double _timeImuTrans; ///< time of current IMU transformation information + + bool _newCornerPointsSharp; ///< flag if a new sharp corner cloud has been received + bool _newCornerPointsLessSharp; ///< flag if a new less sharp corner cloud has been received + bool _newSurfPointsFlat; ///< flag if a new flat surface cloud has been received + bool _newSurfPointsLessFlat; ///< flag if a new less flat surface cloud has been received + bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received + bool _newImuTrans; ///< flag if a new IMU transformation information cloud has been received + + nav_msgs::Odometry _laserOdometry; ///< laser odometry message + tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation + + size_t _lastCornerCloudSize; ///< size of the last corner cloud + size_t _lastSurfaceCloudSize; ///< size of the last surface cloud + + std::vector _pointSearchCornerInd1; ///< first corner point search index buffer + std::vector _pointSearchCornerInd2; ///< second corner point search index buffer + + std::vector _pointSearchSurfInd1; ///< first surface point search index buffer + std::vector _pointSearchSurfInd2; ///< second surface point search index buffer + std::vector _pointSearchSurfInd3; ///< third surface point search index buffer + + Twist _transform; ///< optimized pose transformation + Twist _transformSum; ///< accumulated optimized pose transformation + + Angle _imuRollStart, _imuPitchStart, _imuYawStart; + Angle _imuRollEnd, _imuPitchEnd, _imuYawEnd; + + Vector3 _imuShiftFromStart; + Vector3 _imuVeloFromStart; + + bool _activeMode; ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not) + + ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher + ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher + ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher + ros::Publisher _pubLaserOdometry; ///< laser odometry publisher + tf::TransformBroadcaster _tfBroadcaster; ///< laser odometry transform broadcaster + + ros::Subscriber _subCornerPointsSharp; ///< sharp corner cloud message subscriber + ros::Subscriber _subCornerPointsLessSharp; ///< less sharp corner cloud message subscriber + ros::Subscriber _subSurfPointsFlat; ///< flat surface cloud message subscriber + ros::Subscriber _subSurfPointsLessFlat; ///< less flat surface cloud message subscriber + ros::Subscriber _subLaserCloudFullRes; ///< full resolution cloud message subscriber + ros::Subscriber _subImuTrans; ///< IMU transformation information message subscriber +}; + +} // end namespace loam + +#endif //LOAM_LASERODOMETRY_H diff --git a/src/lib/ScanRegistration.h b/src/lib/ScanRegistration.h index db6f677..008d418 100644 --- a/src/lib/ScanRegistration.h +++ b/src/lib/ScanRegistration.h @@ -246,7 +246,7 @@ typedef struct IMUState { -/** \brief Base class for scan registration implementations. +/** \brief Base class for LOAM scan registration implementations. * * As there exist various sensor devices, producing differently formatted point clouds, * specific implementations are needed for each group of sensor devices to achieve an accurate registration. diff --git a/src/lib/VelodyneScanRegistration.h b/src/lib/VelodyneScanRegistration.h index 4e4434e..f0852e2 100644 --- a/src/lib/VelodyneScanRegistration.h +++ b/src/lib/VelodyneScanRegistration.h @@ -39,6 +39,9 @@ namespace loam { +/** \brief Class for registering Velodyne VLP-16 scans. + * + */ class VelodyneScanRegistration : virtual public ScanRegistration { public: VelodyneScanRegistration(const float& scanPeriod,