Skip to content

Instantly share code, notes, and snippets.

@LimHyungTae

LimHyungTae/service_scans_and_poses.cpp

Created April 27, 2022 02:26
Show Gist options
  • Save LimHyungTae/a305b7d7be666244e3ec9adba1cb3800 to your computer and use it in GitHub Desktop.
Save LimHyungTae/a305b7d7be666244e3ec9adba1cb3800 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
service_scans_and_poses.cpp
#include "utility.h"
#include "lio_sam_de/cloud_info.h"
#include "lio_sam_de/save_map.h"
#include "de_msg/node.h"
#include <iomanip> // for std::setprecision()
//#include <pcl/common/transforms.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
*/
struct PointXYZIRPYT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x)(float, y, y)
(float, z, z)(float, intensity, intensity)
(float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)
(double, time, time))
typedef PointXYZIRPYT PointTypePose;
class mapOptimization : public ParamServer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// gtsam
NonlinearFactorGraph gtSAMgraph;
Values initialEstimate;
Values optimizedEstimate;
ISAM2 *isam;
Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance;
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubDeskewLaserCloud;
ros::Publisher pubLaserOdometryGlobal;
ros::Publisher pubNode;
ros::Publisher pubLaserOdometryIncremental;
ros::Publisher pubKeyPoses;
ros::Publisher pubPath;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Publisher pubLoopConstraintEdge;
ros::Subscriber subCloud;
ros::Subscriber subGPS;
ros::Subscriber subLoop;
ros::ServiceServer srvSaveMap;
std::deque<nav_msgs::Odometry> gpsQueue;
lio_sam_de::cloud_info cloudInfo;
PointTypePose poseWrtInit;
vector<pcl::PointCloud<PointType>::Ptr> deskewCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
vector<double> timestamps;
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;
pcl::PointCloud<PointType>::Ptr laserCloudDeskewLast; // Deskew
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudDeskewLastDS; // downsampled raw point cloud
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
std::vector<PointType> coeffSelCornerVec;
std::vector<bool> laserCloudOriCornerFlag;
std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
std::vector<PointType> coeffSelSurfVec;
std::vector<bool> laserCloudOriSurfFlag;
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
pcl::ApproximateVoxelGrid<PointType> downSizeFilterDeskew;
pcl::ApproximateVoxelGrid<PointType> downSizeFilterCorner;
pcl::ApproximateVoxelGrid<PointType> downSizeFilterSurf;
pcl::ApproximateVoxelGrid<PointType> downSizeFilterICP;
pcl::ApproximateVoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
ros::Time timeLaserInfoStamp;
double timeLaserInfoCur;
double timeLaserInfoCurToSave;
float transformTobeMapped[6];
std::mutex mtx;
std::mutex mtxLoopInfo;
bool isDegenerate = false;
cv::Mat matP;
int laserCloudCornerFromMapDSNum = 0;
int laserCloudSurfFromMapDSNum = 0;
int laserCloudCornerLastDSNum = 0;
int laserCloudSurfLastDSNum = 0;
bool aLoopIsClosed = false;
map<int, int> loopIndexContainer; // from new to old
vector<pair<int, int>> loopIndexQueue;
vector<gtsam::Pose3> loopPoseQueue;
vector<gtsam::noiseModel::Base::shared_ptr> loopNoiseQueue;
deque<std_msgs::Float64MultiArray> loopInfoVec;
nav_msgs::Path globalPath;
Eigen::Affine3f transPointAssociateToMap;
Eigen::Affine3f incrementalOdometryAffineFront;
Eigen::Affine3f incrementalOdometryAffineBack;
mapOptimization() {
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
// pubDeskewLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/deskew_local", 1);
pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry", 1);
pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 1);
pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
pubNode = nh.advertise<de_msg::node>("lio_sam/lastmile/node", 1);
subCloud = nh.subscribe<lio_sam_de::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this,
ros::TransportHints().tcpNoDelay());
subGPS = nh.subscribe<nav_msgs::Odometry>(gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this,
ros::TransportHints().tcpNoDelay());
srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
downSizeFilterDeskew.setLeafSize(0.05, 0.05, 0.05);
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity,
surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
allocateMemory();
}
void allocateMemory() {
cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
// Deskew for static map building
laserCloudDeskewLast.reset(new pcl::PointCloud<PointType>());
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
laserCloudDeskewLastDS.reset(new pcl::PointCloud<PointType>());
laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
for (int i = 0; i < 6; ++i) {
transformTobeMapped[i] = 0;
}
matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
}
void laserCloudInfoHandler(const lio_sam_de::cloud_infoConstPtr &msgIn) {
// extract time stamp
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
timeLaserInfoCurToSave = msgIn->header.stamp.toNSec();
// extract info and feature cloud
cloudInfo = *msgIn;
pcl::fromROSMsg(msgIn->cloud_deskewed, *laserCloudDeskewLast);
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
std::lock_guard<std::mutex> lock(mtx);
static double timeLastProcessing = -1;
updateInitialGuess();
static bool isUpdated = false;
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserInfoCur;
// updateInitialGuess();
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
// ht: At initialization, factors are just added
// Otherwise, factors are saved if the rel pose difference is large enough
saveKeyFramesAndFactor();
correctPoses();
publishOdometry();
publishFrames();
isUpdated = true;
}
if (isUpdated && cloudInfo.odomAvailable && cloudInfo.imuAvailable && !cloudKeyPoses3D->empty() && useNodeInMapOptimization)
publishLastMileNode();
}
void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg) {
gpsQueue.push_back(*gpsMsg);
}
void pointAssociateToMap(PointType const *const pi, PointType *const po) {
po->x =
transPointAssociateToMap(0, 0) * pi->x + transPointAssociateToMap(0, 1) * pi->y + transPointAssociateToMap(0, 2) * pi->z +
transPointAssociateToMap(0, 3);
po->y =
transPointAssociateToMap(1, 0) * pi->x + transPointAssociateToMap(1, 1) * pi->y + transPointAssociateToMap(1, 2) * pi->z +
transPointAssociateToMap(1, 3);
po->z =
transPointAssociateToMap(2, 0) * pi->x + transPointAssociateToMap(2, 1) * pi->y + transPointAssociateToMap(2, 2) * pi->z +
transPointAssociateToMap(2, 3);
po->intensity = pi->intensity;
}
pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn) {
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
int cloudSize = cloudIn->size();
cloudOut->resize(cloudSize);
Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll,
transformIn->pitch, transformIn->yaw);
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < cloudSize; ++i) {
const auto &pointFrom = cloudIn->points[i];
cloudOut->points[i].x =
transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
cloudOut->points[i].y =
transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
cloudOut->points[i].z =
transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
cloudOut->points[i].intensity = pointFrom.intensity;
}
return cloudOut;
}
gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) {
return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
}
gtsam::Pose3 trans2gtsamPose(float transformIn[]) {
return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
}
Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint) {
return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
}
Eigen::Affine3f trans2Affine3f(float transformIn[]) {
return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
}
PointTypePose trans2PointTypePose(float transformIn[]) {
PointTypePose thisPose6D;
thisPose6D.x = transformIn[3];
thisPose6D.y = transformIn[4];
thisPose6D.z = transformIn[5];
thisPose6D.roll = transformIn[0];
thisPose6D.pitch = transformIn[1];
thisPose6D.yaw = transformIn[2];
return thisPose6D;
}
bool saveMapService(lio_sam_de::save_mapRequest &req, lio_sam_de::save_mapResponse &res) {
string saveMapDirectory;
cout << "****************************************************" << endl;
cout << extrinsicLiDAR2BodyRot << endl;
cout << extrinsicLiDAR2BodyTrans << endl;
cout << "Saving map to pcd files ..." << endl;
if (req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
else saveMapDirectory = std::getenv("HOME") + req.destination;
cout << "Save destination: " << saveMapDirectory << endl;
// create directory and remove old files;
int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
// save key frame transformations
pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
// extract global point cloud map
pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
cout << "processing start" << endl;
for (int i = 0; i < (int) cloudKeyPoses3D->size(); i++) {
auto wrtInit = pclPointToAffine3f(cloudKeyPoses6D->points[0]).inverse() * pclPointToAffine3f(cloudKeyPoses6D->points[i]);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(wrtInit, x, y, z, roll, pitch, yaw);
poseWrtInit.x = x;
poseWrtInit.y = y;
poseWrtInit.z = z;
poseWrtInit.roll = roll;
poseWrtInit.pitch = pitch;
poseWrtInit.yaw = yaw;
*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &poseWrtInit);
*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &poseWrtInit);
cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
}
cout << "processing end" << endl;
Eigen::Matrix4f lidar2sensorbox;
lidar2sensorbox.block<3, 3>(0, 0) = extrinsicLiDAR2BodyRot.template cast<float>();
lidar2sensorbox.block<3, 1>(0, 3) = extrinsicLiDAR2BodyTrans.template cast<float>().transpose();
std::cout<<lidar2sensorbox<<std::endl;
saveEachPosesAndDeskewedPcs(saveMapDirectory, lidar2sensorbox);
if (req.resolution != 0) {
cout << "\n\nSave resolution: " << req.resolution << endl;
// down-sample and save corner cloud
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
downSizeFilterCorner.filter(*globalCornerCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
// down-sample and save surf cloud
downSizeFilterSurf.setInputCloud(globalSurfCloud);
downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);
downSizeFilterSurf.filter(*globalSurfCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
} else {
// save corner cloud
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
// save surf cloud
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
}
// save global point cloud map
*globalMapCloud += *globalCornerCloud;
*globalMapCloud += *globalSurfCloud;
// pcl::PointCloud<PointType>::Ptr globalMapWrtBox(new pcl::PointCloud<PointType>());
// pcl::transformPointCloud(*globalMapCloud, *globalMapWrtBox, lidar2box.inverse());
int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
res.success = ret == 0;
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
cout << "****************************************************" << endl;
cout << "Saving map to pcd files completed\n" << endl;
return true;
}
void visualizeGlobalMapThread() {
ros::Rate rate(0.2);
while (ros::ok()) {
rate.sleep();
publishGlobalMap();
}
if (savePCD == false)
return;
lio_sam_de::save_mapRequest req;
lio_sam_de::save_mapResponse res;
// if(!saveMapService(req, res)){
// cout << "Fail to save map" << endl;
//}
}
void publishGlobalMap() {
if (pubLaserCloudSurround.getNumSubscribers() == 0)
return;
if (cloudKeyPoses3D->points.empty() == true)
return;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
// kd-tree to find near key frames to visualize
std::vector<int> pointSearchIndGlobalMap;
std::vector<float> pointSearchSqDisGlobalMap;
// search near key frames to visualize
mtx.lock();
kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap,
pointSearchSqDisGlobalMap, 0);
mtx.unlock();
for (int i = 0; i < (int) pointSearchIndGlobalMap.size(); ++i)
globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
// downsample near selected key frames
pcl::ApproximateVoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity,
globalMapVisualizationPoseDensity); // for global map visualization
downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
for (auto &pt : globalMapKeyPosesDS->points) {
kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
}
// extract visualized and downsampled key frames
for (int i = 0; i < (int) globalMapKeyPosesDS->size(); ++i) {
if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
continue;
int thisKeyInd = (int) globalMapKeyPosesDS->points[i].intensity;
*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
}
// downsample visualized points
pcl::ApproximateVoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize,
globalMapVisualizationLeafSize); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
}
void saveEachPosesAndDeskewedPcs(string &mapdir, const Eigen::Matrix4f &lidar2box) {
string filename = (boost::format("%s/poses_lio2box.csv") % mapdir).str();
ofstream poses_txt(filename);
poses_txt << "index, timestamp, x, y, z, qx, qy, qz, qw" << "\n";
if (timestamps.size() != cloudKeyPoses6D->size()) {
cout << timestamps.size() << " vs " << cloudKeyPoses6D->size() << endl;
throw invalid_argument("[saveEachPoses] Sizes are not equal!!");
}
if (deskewCloudKeyFrames.size() != cloudKeyPoses6D->size()) {
cout << deskewCloudKeyFrames.size() << " vs " << cloudKeyPoses6D->size() << endl;
throw invalid_argument("[saveEachPoses] Deskew Cloud Sizes are not equal!!");
}
pcl::PointCloud<PointType>::Ptr globalMapWrtBox(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapWrtBoxDS(new pcl::PointCloud<PointType>());
// Save each keyframe point cloud
std::string pcddir = mapdir + "pcds/";
int unused = system((std::string("exec rm -r ") + pcddir).c_str());
unused = system((std::string("mkdir -p ") + pcddir).c_str());
for (int i = 0; i < (int) cloudKeyPoses6D->size(); i++) {
auto wrtInit = pclPointToAffine3f(cloudKeyPoses6D->points[0]).inverse() * pclPointToAffine3f(cloudKeyPoses6D->points[i]);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(wrtInit, x, y, z, roll, pitch, yaw);
poseWrtInit.x = x;
poseWrtInit.y = y;
poseWrtInit.z = z;
poseWrtInit.roll = roll;
poseWrtInit.pitch = pitch;
poseWrtInit.yaw = yaw;
const auto &p = poseWrtInit;
tf::Quaternion q;
q.setRPY(p.roll, p.pitch, p.yaw);
q.normalize();
// poses_txt << i << ", " << timestamps[i] << ", " << p.x << ", " << p.y << ", " << p.z
// << ", "
// << q.getX() << ", " << q.getY() << ", " << q.getZ() << ", " << q.getW()
// << "\n";
// Pose: sensor box
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
pose(0, 3) = p.x;
pose(1, 3) = p.y;
pose(2, 3) = p.z;
tf::Matrix3x3 rot;
rot.setRotation(q);
auto rotation = rotMat2EigenRot(rot);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
pose(i, j) = rotation(i, j);
}
}
Eigen::Matrix4f poseWrtBox = pose * lidar2box;
Eigen::Quaternionf q_tf(poseWrtBox.block<3, 3>(0, 0));
std::cout << std::setprecision(18);
// cout<< timestamps[i] <<endl;
poses_txt << std::setprecision(18) << i << ", " << (timestamps[i]) << ", " << poseWrtBox(0, 3) << ", " << poseWrtBox(1, 3)
<< ", " << poseWrtBox(2, 3)
<< ", " << q_tf.x() << ", " << q_tf.y() << ", " << q_tf.z() << ", " << q_tf.w() << "\n";
pcl::PointCloud<PointType>::Ptr deskewWrtBox(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*deskewCloudKeyFrames[i], *deskewWrtBox, lidar2box.inverse());
string output_name = (boost::format("%s/%06d.pcd") % pcddir % i).str();
pcl::io::savePCDFileASCII(output_name, *deskewWrtBox);
pcl::PointCloud<PointType>::Ptr deskewGlobal(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*deskewWrtBox, *deskewGlobal, poseWrtBox);
*globalMapWrtBox += *deskewGlobal;
}
downSizeFilterDeskew.setInputCloud(globalMapWrtBox);
downSizeFilterDeskew.filter(*globalMapWrtBoxDS);
string dense_name = (boost::format("%s/dense_global_map.pcd") % mapdir).str();
pcl::io::savePCDFileASCII(dense_name, *globalMapWrtBoxDS);
poses_txt.close();
}
Eigen::Matrix3f rotMat2EigenRot(const tf::Matrix3x3 &rotMat) {
Eigen::Matrix3f eigenRotMat(3, 3);
eigenRotMat(0, 0) = rotMat[0][0];
eigenRotMat(0, 1) = rotMat[0][1];
eigenRotMat(0, 2) = rotMat[0][2];
eigenRotMat(1, 0) = rotMat[1][0];
eigenRotMat(1, 1) = rotMat[1][1];
eigenRotMat(1, 2) = rotMat[1][2];
eigenRotMat(2, 0) = rotMat[2][0];
eigenRotMat(2, 1) = rotMat[2][1];
eigenRotMat(2, 2) = rotMat[2][2];
return eigenRotMat;
}
void loopClosureThread() {
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(loopClosureFrequency);
while (ros::ok()) {
rate.sleep();
performLoopClosure();
visualizeLoopClosure();
}
}
void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr &loopMsg) {
std::lock_guard<std::mutex> lock(mtxLoopInfo);
if (loopMsg->data.size() != 2)
return;
loopInfoVec.push_back(*loopMsg);
while (loopInfoVec.size() > 5)
loopInfoVec.pop_front();
}
void performLoopClosure() {
if (cloudKeyPoses3D->points.empty() == true)
return;
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
// find keys
int loopKeyCur;
int loopKeyPre;
if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
return;
// extract cloud
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
{
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
// ICP Settings
static pcl::GeneralizedIterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(2.0);
icp.setMaximumIterations(250);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// Align clouds
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// publish corrected cloud
if (pubIcpKeyFrames.getNumSubscribers() != 0) {
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// Get pose transformation
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
correctionLidarFrame = icp.getFinalTransformation();
// transform from world origin to wrong pose
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// transform from world origin to corrected pose
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
// noise is replaced as follows:
// Since icp sometimes fails!
float noiseScore = 0.5; //icp.getFitnessScore();
float noise = 0.0001;
Vector6 << noiseScore, noiseScore, noise, noise, noise, noiseScore;
gtsam::noiseModel::Base::shared_ptr robustNoiseModel = gtsam::noiseModel::Robust::Create(
gtsam::noiseModel::mEstimator::DCS::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure
gtsam::noiseModel::Diagonal::Variances(Vector6)
);
// Add pose constraint
mtx.lock();
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(robustNoiseModel);
mtx.unlock();
// add loop constriant
loopIndexContainer[loopKeyCur] = loopKeyPre;
}
bool detectLoopClosureDistance(int *latestID, int *closestID) {
int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
int loopKeyPre = -1;
// check loop constraint added before
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
// find the closest history key frame
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop,
pointSearchSqDisLoop, 0);
for (int i = 0; i < (int) pointSearchIndLoop.size(); ++i) {
int id = pointSearchIndLoop[i];
if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff) {
loopKeyPre = id;
break;
}
}
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}
bool detectLoopClosureExternal(int *latestID, int *closestID) {
// this function is not used yet, please ignore it
int loopKeyCur = -1;
int loopKeyPre = -1;
std::lock_guard<std::mutex> lock(mtxLoopInfo);
if (loopInfoVec.empty())
return false;
double loopTimeCur = loopInfoVec.front().data[0];
double loopTimePre = loopInfoVec.front().data[1];
loopInfoVec.pop_front();
if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
return false;
int cloudSize = copy_cloudKeyPoses6D->size();
if (cloudSize < 2)
return false;
// latest key
loopKeyCur = cloudSize - 1;
for (int i = cloudSize - 1; i >= 0; --i) {
if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
else
break;
}
// previous key
loopKeyPre = 0;
for (int i = 0; i < cloudSize; ++i) {
if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
else
break;
}
if (loopKeyCur == loopKeyPre)
return false;
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr &nearKeyframes, const int &key, const int &searchNum) {
// extract near keyframes
nearKeyframes->clear();
int cloudSize = copy_cloudKeyPoses6D->size();
for (int i = -searchNum; i <= searchNum; ++i) {
int keyNear = key + i;
if (keyNear < 0 || keyNear >= cloudSize)
continue;
*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
}
if (nearKeyframes->empty())
return;
// downsample near keyframes
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterICP.setInputCloud(nearKeyframes);
downSizeFilterICP.filter(*cloud_temp);
*nearKeyframes = *cloud_temp;
}
void visualizeLoopClosure() {
if (loopIndexContainer.empty())
return;
visualization_msgs::MarkerArray markerArray;
// loop nodes
visualization_msgs::Marker markerNode;
markerNode.header.frame_id = odometryFrame;
markerNode.header.stamp = timeLaserInfoStamp;
markerNode.action = visualization_msgs::Marker::ADD;
markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
markerNode.ns = "loop_nodes";
markerNode.id = 0;
markerNode.pose.orientation.w = 1;
markerNode.scale.x = 0.3;
markerNode.scale.y = 0.3;
markerNode.scale.z = 0.3;
markerNode.color.r = 0;
markerNode.color.g = 0.8;
markerNode.color.b = 1;
markerNode.color.a = 1;
// loop edges
visualization_msgs::Marker markerEdge;
markerEdge.header.frame_id = odometryFrame;
markerEdge.header.stamp = timeLaserInfoStamp;
markerEdge.action = visualization_msgs::Marker::ADD;
markerEdge.type = visualization_msgs::Marker::LINE_LIST;
markerEdge.ns = "loop_edges";
markerEdge.id = 1;
markerEdge.pose.orientation.w = 1;
markerEdge.scale.x = 0.1;
markerEdge.color.r = 0.9;
markerEdge.color.g = 0.9;
markerEdge.color.b = 0;
markerEdge.color.a = 1;
for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it) {
int key_cur = it->first;
int key_pre = it->second;
geometry_msgs::Point p;
p.x = copy_cloudKeyPoses6D->points[key_cur].x;
p.y = copy_cloudKeyPoses6D->points[key_cur].y;
p.z = copy_cloudKeyPoses6D->points[key_cur].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
p.x = copy_cloudKeyPoses6D->points[key_pre].x;
p.y = copy_cloudKeyPoses6D->points[key_pre].y;
p.z = copy_cloudKeyPoses6D->points[key_pre].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
}
markerArray.markers.push_back(markerNode);
markerArray.markers.push_back(markerEdge);
pubLoopConstraintEdge.publish(markerArray);
}
void updateInitialGuess() {
// save current transformation before any processing
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
static Eigen::Affine3f lastImuTransformation;
// initialization
if (cloudKeyPoses3D->points.empty()) {
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (!useImuHeadingInitialization)
transformTobeMapped[2] = 0;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
cloudInfo.imuYawInit); // save imu before return;
return;
}
// use imu pre-integration estimation for pose guess
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
// ht: `true` means that the pose from the `startOdomMsg` in imageProjection.cpp is valid
// ht: imuAvailable is always `true`, yet odomAvailable is set to `true` after a while
if (cloudInfo.odomAvailable == true) {
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch,
cloudInfo.initialGuessYaw);
if (lastImuPreTransAvailable == false) {
lastImuPreTransformation = transBack;
lastImuPreTransAvailable = true;
} else {
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Eigen::Affine3f transFinal = transTobe * transIncre;
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuPreTransformation = transBack;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
cloudInfo.imuYawInit); // save imu before return;
return;
}
}
// use imu incremental estimation for pose guess (only rotation)
if (cloudInfo.imuAvailable == true) {
Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
cloudInfo.imuYawInit);
Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Eigen::Affine3f transFinal = transTobe * transIncre;
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
cloudInfo.imuYawInit); // save imu before return;
return;
}
}
void extractForLoopClosure() {
pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
int numPoses = cloudKeyPoses3D->size();
for (int i = numPoses - 1; i >= 0; --i) {
if ((int) cloudToExtract->size() <= surroundingKeyframeSize)
cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
else
break;
}
extractCloud(cloudToExtract);
}
void extractNearby() {
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// extract all the nearby key poses and downsample them
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double) surroundingKeyframeSearchRadius, pointSearchInd,
pointSearchSqDis);
for (int i = 0; i < (int) pointSearchInd.size(); ++i) {
int id = pointSearchInd[i];
surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
}
// HT: By doing so, redundant keyframes are filtered, i.e. the case where a robot stop for a while
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
for (auto &pt : surroundingKeyPosesDS->points) {
kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
}
// also extract some latest key frames in case the robot rotates in one position
int numPoses = cloudKeyPoses3D->size();
for (int i = numPoses - 1; i >= 0; --i) {
if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
else
break;
}
extractCloud(surroundingKeyPosesDS);
}
void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract) {
// fuse the map
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < (int) cloudToExtract->size(); ++i) {
if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
continue;
int thisKeyInd = (int) cloudToExtract->points[i].intensity;
// HT: laserCloudMapContainer - map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>>
if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) {
// transformed cloud available
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
} else {
// transformed cloud not available
pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],
&cloudKeyPoses6D->points[thisKeyInd]);
pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],
&cloudKeyPoses6D->points[thisKeyInd]);
*laserCloudCornerFromMap += laserCloudCornerTemp;
*laserCloudSurfFromMap += laserCloudSurfTemp;
laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
}
}
// Downsample the surrounding corner key frames (or map)
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
// Downsample the surrounding surf key frames (or map)
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
// clear map cache if too large
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}
void extractSurroundingKeyFrames() {
if (cloudKeyPoses3D->points.empty() == true)
return;
// if (loopClosureEnableFlag == true)
// {
// extractForLoopClosure();
// } else {
// extractNearby();
// }
extractNearby();
}
void downsampleCurrentScan() {
laserCloudDeskewLastDS->clear();
downSizeFilterDeskew.setInputCloud(laserCloudDeskewLast);
downSizeFilterDeskew.filter(*laserCloudDeskewLastDS);
// Downsample cloud from current scan
laserCloudCornerLastDS->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerLastDS);
laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
laserCloudSurfLastDS->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfLastDS);
laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}
void updatePointAssociateToMap() {
transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
}
void cornerOptimization() {
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudCornerLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5;
cy /= 5;
cz /= 5;
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax;
a12 += ax * ay;
a13 += ax * az;
a22 += ay * ay;
a23 += ay * az;
a33 += az * az;
}
a11 /= 5;
a12 /= 5;
a13 /= 5;
a22 /= 5;
a23 /= 5;
a33 /= 5;
matA1.at<float>(0, 0) = a11;
matA1.at<float>(0, 1) = a12;
matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12;
matA1.at<float>(1, 1) = a22;
matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13;
matA1.at<float>(2, 1) = a23;
matA1.at<float>(2, 2) = a33;
cv::eigen(matA1, matD1, matV1);
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
+ ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))
+ ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)));
float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
float la = ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
+ (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / a012 / l12;
float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
- (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))
+ (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
float ld2 = a012 / l12;
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1) {
laserCloudOriCornerVec[i] = pointOri;
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
void surfOptimization() {
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++) {
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
matA0.setZero();
matB0.fill(-1);
matX0.setZero();
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
matX0 = matA0.colPivHouseholderQr().solve(matB0);
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
bool planeValid = true;
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->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 s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
void combineOptimizationCoeffs() {
// combine corner coeffs
for (int i = 0; i < laserCloudCornerLastDSNum; ++i) {
if (laserCloudOriCornerFlag[i] == true) {
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
}
}
// combine surf coeffs
for (int i = 0; i < laserCloudSurfLastDSNum; ++i) {
if (laserCloudOriSurfFlag[i] == true) {
laserCloudOri->push_back(laserCloudOriSurfVec[i]);
coeffSel->push_back(coeffSelSurfVec[i]);
}
}
// reset flag for next iteration
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}
bool LMOptimization(int iterCount) {
// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
float srx = sin(transformTobeMapped[1]);
float crx = cos(transformTobeMapped[1]);
float sry = sin(transformTobeMapped[2]);
float cry = cos(transformTobeMapped[2]);
float srz = sin(transformTobeMapped[0]);
float crz = cos(transformTobeMapped[0]);
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
PointType pointOri, coeff;
for (int i = 0; i < laserCloudSelNum; i++) {
// lidar -> camera
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// lidar -> camera
coeff.x = coeffSel->points[i].y;
coeff.y = coeffSel->points[i].z;
coeff.z = coeffSel->points[i].x;
coeff.intensity = coeffSel->points[i].intensity;
// in camera
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;
// lidar -> camera
matA.at<float>(i, 0) = arz;
matA.at<float>(i, 1) = arx;
matA.at<float>(i, 2) = ary;
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;
matB.at<float>(i, 0) = -coeff.intensity;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
if (iterCount == 0) {
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));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(3, 0) * 100, 2) +
pow(matX.at<float>(4, 0) * 100, 2) +
pow(matX.at<float>(5, 0) * 100, 2));
if (deltaR < 0.05 && deltaT < 0.05) {
return true; // converged
}
return false; // keep optimizing
}
void scan2MapOptimization() {
if (cloudKeyPoses3D->points.empty())
return;
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum) {
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 30; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization();
surfOptimization();
combineOptimizationCoeffs();
if (LMOptimization(iterCount) == true)
break;
}
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum,
laserCloudSurfLastDSNum);
}
}
void transformUpdate() {
if (cloudInfo.imuAvailable == true) {
if (std::abs(cloudInfo.imuPitchInit) < 1.4) {
double imuWeight = imuRPYWeight;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[0] = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[1] = pitchMid;
}
}
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
}
float constraintTransformation(float value, float limit) {
if (value < -limit)
value = -limit;
if (value > limit)
value = limit;
return value;
}
bool saveFrame() {
if (cloudKeyPoses3D->points.empty())
return true;
Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
return false;
return true;
}
void addOdomFactor() {
if (cloudKeyPoses3D->points.empty()) {
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
} else {
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
gtSAMgraph.add(
BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
}
}
void addGPSFactor() {
if (gpsQueue.empty())
return;
// wait for system initialized and settles down
if (cloudKeyPoses3D->points.empty())
return;
else {
if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
return;
}
// pose covariance small, no need to correct
if (poseCovariance(3, 3) < poseCovThreshold && poseCovariance(4, 4) < poseCovThreshold)
return;
// last gps position
static PointType lastGPSPoint;
while (!gpsQueue.empty()) {
if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2) {
// message too old
gpsQueue.pop_front();
} else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2) {
// message too new
break;
} else {
nav_msgs::Odometry thisGPS = gpsQueue.front();
gpsQueue.pop_front();
// GPS too noisy, skip
float noise_x = thisGPS.pose.covariance[0];
float noise_y = thisGPS.pose.covariance[7];
float noise_z = thisGPS.pose.covariance[14];
if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
continue;
float gps_x = thisGPS.pose.pose.position.x;
float gps_y = thisGPS.pose.pose.position.y;
float gps_z = thisGPS.pose.pose.position.z;
if (!useGpsElevation) {
gps_z = transformTobeMapped[5];
noise_z = 0.01;
}
// GPS not properly initialized (0,0,0)
if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
continue;
// Add GPS every a few meters
PointType curGPSPoint;
curGPSPoint.x = gps_x;
curGPSPoint.y = gps_y;
curGPSPoint.z = gps_z;
if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
continue;
else
lastGPSPoint = curGPSPoint;
gtsam::Vector Vector3(3);
Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
gtSAMgraph.add(gps_factor);
aLoopIsClosed = true;
break;
}
}
}
void addLoopFactor() {
if (loopIndexQueue.empty())
return;
for (int i = 0; i < (int) loopIndexQueue.size(); ++i) {
int indexFrom = loopIndexQueue[i].first;
int indexTo = loopIndexQueue[i].second;
gtsam::Pose3 poseBetween = loopPoseQueue[i];
auto noiseBetween = loopNoiseQueue[i];
gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
}
loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
aLoopIsClosed = true;
}
void saveKeyFramesAndFactor() {
if (saveFrame() == false)
return;
// odom factor
addOdomFactor();
// gps factor
addGPSFactor();
// loop factor
addLoopFactor();
// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");
// update iSAM
isam->update(gtSAMgraph, initialEstimate);
isam->update();
if (aLoopIsClosed == true) {
isam->update();
isam->update();
isam->update();
isam->update();
isam->update();
}
gtSAMgraph.resize(0);
initialEstimate.clear();
//save key poses
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1);
// cout << "****************************************************" << endl;
// isamCurrentEstimate.print("Current estimate: ");
thisPose3D.x = latestEstimate.translation().x();
thisPose3D.y = latestEstimate.translation().y();
thisPose3D.z = latestEstimate.translation().z();
thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().roll();
thisPose6D.pitch = latestEstimate.rotation().pitch();
thisPose6D.yaw = latestEstimate.rotation().yaw();
thisPose6D.time = timeLaserInfoCur;
cloudKeyPoses6D->push_back(thisPose6D);
// cout << "****************************************************" << endl;
// cout << "Pose covariance:" << endl;
// cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);
// save updated transform
transformTobeMapped[0] = latestEstimate.rotation().roll();
transformTobeMapped[1] = latestEstimate.rotation().pitch();
transformTobeMapped[2] = latestEstimate.rotation().yaw();
transformTobeMapped[3] = latestEstimate.translation().x();
transformTobeMapped[4] = latestEstimate.translation().y();
transformTobeMapped[5] = latestEstimate.translation().z();
// save all the received edge and surf points
pcl::PointCloud<PointType>::Ptr thisDeskewKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudDeskewLastDS, *thisDeskewKeyFrame);
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
// save key frame cloud
if (saveRawPointCloud) {
deskewCloudKeyFrames.push_back(thisDeskewKeyFrame);
}
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
timestamps.push_back(timeLaserInfoCurToSave);
// save path for visualization
updatePath(thisPose6D);
}
void correctPoses() {
if (cloudKeyPoses3D->points.empty())
return;
if (aLoopIsClosed == true) {
// clear map cache
laserCloudMapContainer.clear();
// clear path
globalPath.poses.clear();
// update key poses
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i) {
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
updatePath(cloudKeyPoses6D->points[i]);
}
aLoopIsClosed = false;
}
}
void updatePath(const PointTypePose &pose_in) {
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose.position.x = pose_in.x;
pose_stamped.pose.position.y = pose_in.y;
pose_stamped.pose.position.z = pose_in.z;
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
pose_stamped.pose.orientation.x = q.x();
pose_stamped.pose.orientation.y = q.y();
pose_stamped.pose.orientation.z = q.z();
pose_stamped.pose.orientation.w = q.w();
globalPath.poses.push_back(pose_stamped);
}
void publishLastMileNode() {
/***
* For Lastmile: publish node
*/
de_msg::node node;
geometry_msgs::Pose poseLast;
poseLast.position.x = transformTobeMapped[3];
poseLast.position.y = transformTobeMapped[4];
poseLast.position.z = transformTobeMapped[5];
poseLast.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0],
transformTobeMapped[1],
transformTobeMapped[2]);
static int idx = 0;
pcl::toROSMsg(*laserCloudDeskewLast, node.laser);
node.idx.data = idx++;
node.odom = poseLast;
node.header.seq = node.idx.data;
node.header.stamp = timeLaserInfoStamp;
node.header.frame_id = odometryFrame;
// cout << "[LAST-MILE] " << idx << "-th node is published!" << endl;
pubNode.publish(node);
}
void publishOdometry() {
// Publish odometry for ROS (global)
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = timeLaserInfoStamp;
laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0],
transformTobeMapped[1],
transformTobeMapped[2]);
pubLaserOdometryGlobal.publish(laserOdometryROS);
// Publish TF
static tf::TransformBroadcaster br;
tf::Transform t_odom_to_lidar = tf::Transform(
tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame,
"lidar_link");
br.sendTransform(trans_odom_to_lidar);
// Publish odometry for ROS (incremental)
static bool lastIncreOdomPubFlag = false;
static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
if (lastIncreOdomPubFlag == false) {
lastIncreOdomPubFlag = true;
laserOdomIncremental = laserOdometryROS;
increOdomAffine = trans2Affine3f(transformTobeMapped);
} else {
Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
increOdomAffine = increOdomAffine * affineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(increOdomAffine, x, y, z, roll, pitch, yaw);
if (cloudInfo.imuAvailable == true) {
if (std::abs(cloudInfo.imuPitchInit) < 1.4) {
double imuWeight = 0.1;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
transformQuaternion.setRPY(roll, 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
roll = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}
laserOdomIncremental.header.stamp = timeLaserInfoStamp;
laserOdomIncremental.header.frame_id = odometryFrame;
laserOdomIncremental.child_frame_id = "odom_mapping";
laserOdomIncremental.pose.pose.position.x = x;
laserOdomIncremental.pose.pose.position.y = y;
laserOdomIncremental.pose.pose.position.z = z;
laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
if (isDegenerate)
laserOdomIncremental.pose.covariance[0] = 1;
else
laserOdomIncremental.pose.covariance[0] = 0;
}
pubLaserOdometryIncremental.publish(laserOdomIncremental);
}
void publishFrames() {
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// publish registered key frame
if (pubRecentKeyFrame.getNumSubscribers() != 0) {
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish registered high-res raw cloud
if (pubCloudRegisteredRaw.getNumSubscribers() != 0) {
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
// *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish path
if (pubPath.getNumSubscribers() != 0) {
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = odometryFrame;
pubPath.publish(globalPath);
}
}
};
int main(int argc, char **argv) {
ros::init(argc, argv, "lio_sam");
mapOptimization MO;
ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::spin();
loopthread.join();
visualizeMapThread.join();
return 0;
}
@LimHyungTae
Copy link
Author

LimHyungTae commented Apr 27, 2022 •
edited
Loading

  • 86번 째 줄 ros::ServiceServer srvSaveMap; 선언
  • 371번 째 줄 saveMapService 함수 참고
rosservice call /lio_sam/save_map "resolution: 0.0
destination: ''"

와 같이 service를 날리면 save되게 되어 있음

Pose optimization backend로 하는 법

addOdomFactor와 addLoopFactor 참고

전체 과정 정리

  1. Given: N개의 pose와 N개의 scans pairs
  2. 가장 첫 번째 pose는 prior로! Position: (0,0,0), Quaternion (0, 0, 0, 1). 아래와 같이 graph에 PriorFactor를 add함 + initalEstimate에 0번째 pose value insert해줘야 함
if (cloudKeyPoses3D->points.empty()) {
    noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
            (Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
    gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
    initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
  1. 그 이후에는 (i-1) 번째와 i 번째의 FasterLIO로 구한 relative pose를 factor로 넣어줌. 아래의 경우에는 online SLAM이었기 때문에 가장 최신의 odom만 추가해주면 되는데, 우리 같은 경우에는 N-1 for 문을 돌면서 i-1과 i의 BetweenFactor를 추가하고, FasterLIO로 계산한 pose를 initialEstimate에 잘 추가해주어야 함!
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
                    (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            gtsam::Pose3                     poseFrom      = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3                     poseTo        = trans2gtsamPose(transformTobeMapped);
            gtSAMgraph.add(
                    BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
  1. performLoopClosure 함수를 수정해서 N개의 pose의 loop candidates와 계산된 relative pose를 loopIndexQueue와 loopPoseQueue에 담아 둚
  2. addLoopFactor 함수를 참고하여 찾은 loop에 대한 factor들 추가
  3. Optimize!
  4. 그 후, correctPoses 함수를 보면 N개의 corrected poses에 접근하는 예시 나와있음

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment