Skip to content

Commit b19e197

Browse files
author
Vsevolod Hulchuk
committed
separate scan alignment
1 parent ca47ac4 commit b19e197

7 files changed

Lines changed: 448 additions & 405 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ add_executable(${PROJECT_NAME}_mapOptmization
9696
src/mapOptimization/mapOptimization_gps.cpp
9797
src/mapOptimization/mapOptimization_loop.cpp
9898
src/mapOptimization/mapOptimization_publish.cpp
99+
src/scanAlignment/ScanAligner.cpp
99100
${common_lib}
100101
include/Scancontext.cpp)
101102
target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})

‎include/mapOptimization/mapOptimization.hpp‎

Lines changed: 4 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,8 @@
3131

3232
#include "Scancontext.h"
3333
#include "tictoc.h"
34+
#include "scanAlignment/ScanAligner.hpp"
35+
3436
#include <fstream>
3537
#include <iomanip>
3638
#include <sstream>
@@ -113,6 +115,7 @@ class mapOptimization : public ParamServer
113115
{
114116
public:
115117
MapExporter map_exporter_;
118+
std::shared_ptr<ScanAligner> scanAligner;
116119

117120
gtsam::NonlinearFactorGraph gtSAMgraph;
118121
gtsam::Values initialEstimate;
@@ -206,28 +209,6 @@ class mapOptimization : public ParamServer
206209
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;
207210
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS;
208211

209-
pcl::PointCloud<PointType>::Ptr laserCloudOri;
210-
pcl::PointCloud<PointType>::Ptr coeffSel;
211-
212-
std::vector<PointType> laserCloudOriSurfVec;
213-
std::vector<PointType> coeffSelSurfVec;
214-
std::vector<bool> laserCloudOriSurfFlag;
215-
std::vector<uint8_t> laserCloudSurfKnnPassFlag;
216-
std::vector<uint8_t> laserCloudSurfPlaneValidFlag;
217-
std::vector<uint8_t> laserCloudSurfDebugCode;
218-
219-
static constexpr uint8_t SURF_DEBUG_ACCEPTED = 0;
220-
static constexpr uint8_t SURF_DEBUG_REJECTED_NEIGHBOR_COUNT = 1;
221-
static constexpr uint8_t SURF_DEBUG_REJECTED_KNN_DISTANCE = 2;
222-
static constexpr uint8_t SURF_DEBUG_REJECTED_PLANE_INVALID = 3;
223-
static constexpr uint8_t SURF_DEBUG_REJECTED_LOW_WEIGHT = 4;
224-
static constexpr uint8_t SURF_DEBUG_NOT_OPTIMIZED = 5;
225-
226-
uint32_t surfStageInputCount = 0;
227-
uint32_t surfStageKnnPassCount = 0;
228-
uint32_t surfStagePlaneValidCount = 0;
229-
uint32_t surfStageMatchedCount = 0;
230-
231212
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
232213
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
233214
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
@@ -261,7 +242,6 @@ class mapOptimization : public ParamServer
261242
std::mutex mtxLoopInfo;
262243

263244
bool isDegenerate = false;
264-
cv::Mat matP;
265245

266246
int laserCloudSurfFromMapDSNum = 0;
267247
int laserCloudSurfLastDSNum = 0;
@@ -275,7 +255,6 @@ class mapOptimization : public ParamServer
275255

276256
nav_msgs::msg::Path globalPath;
277257

278-
Eigen::Affine3f transPointAssociateToMap;
279258
Eigen::Affine3f incrementalOdometryAffineFront;
280259
Eigen::Affine3f incrementalOdometryAffineBack;
281260
Eigen::Affine3f lastIncrementalDeltaPoseLocal{Eigen::Affine3f::Identity()};
@@ -303,7 +282,7 @@ class mapOptimization : public ParamServer
303282
void initializeDatum(double lat, double lon, double alt, double heading_deg);
304283
void gpsHandler(const sensor_msgs::msg::NavSatFix::SharedPtr gpsMsg);
305284

306-
void pointAssociateToMap(PointType const *const pi, PointType *const po);
285+
// void pointAssociateToMap(PointType const *const pi, PointType *const po);
307286
pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn);
308287
gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint);
309288
gtsam::Pose3 trans2gtsamPose(float transformIn[]);
@@ -340,10 +319,6 @@ class mapOptimization : public ParamServer
340319
void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract);
341320
void extractSurroundingKeyFrames();
342321
void downsampleCurrentScan();
343-
void updatePointAssociateToMap();
344-
void surfOptimization();
345-
void combineOptimizationCoeffs();
346-
bool LMOptimization(int iterCount);
347322
void scan2MapOptimization();
348323
void transformUpdate();
349324
float constraintTransformation(float value, float limit);

‎include/scanAlignment/ScanAligner.hpp‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
// File: include/scanAlignment/ScanAligner.hpp
2+
#pragma once
3+
4+
#include "utility.h"
5+
#include <pcl/common/transforms.h>
6+
#include <opencv2/opencv.hpp>
7+
#include <chrono>
8+
#include "tictoc.h"
9+
10+
struct AlignmentMetrics {
11+
double surf_optimization_ms = 0.0;
12+
double combine_ms = 0.0;
13+
double lm_optimization_ms = 0.0;
14+
int iterations = 0;
15+
int final_correspondences = 0;
16+
uint32_t surf_input_count = 0;
17+
uint32_t surf_knn_pass_count = 0;
18+
uint32_t surf_plane_valid_count = 0;
19+
uint32_t surf_matched_count = 0;
20+
bool is_degenerate = false;
21+
};
22+
23+
class ScanAligner {
24+
private:
25+
pcl::PointCloud<PointType>::Ptr mapCloud;
26+
pcl::KdTreeFLANN<PointType>::Ptr kdtreeMap;
27+
28+
pcl::PointCloud<PointType>::Ptr laserCloudOri;
29+
pcl::PointCloud<PointType>::Ptr coeffSel;
30+
31+
std::vector<PointType> laserCloudOriSurfVec;
32+
std::vector<PointType> coeffSelSurfVec;
33+
std::vector<uint8_t> laserCloudOriSurfFlag;
34+
35+
float currentTransform[6];
36+
Eigen::Affine3f transPointAssociateToMap;
37+
38+
float surfKnnMinDistance;
39+
int numberOfCores;
40+
41+
cv::Mat matP;
42+
bool isDegenerate;
43+
44+
uint32_t surfStageInputCount = 0;
45+
uint32_t surfStageKnnPassCount = 0;
46+
uint32_t surfStagePlaneValidCount = 0;
47+
uint32_t surfStageMatchedCount = 0;
48+
49+
void pointAssociateToMap(PointType const *const pi, PointType *const po);
50+
void updatePointAssociateToMap();
51+
void surfOptimization(const pcl::PointCloud<PointType>::Ptr &scan);
52+
void combineOptimizationCoeffs(int scanSize);
53+
bool LMOptimization(int iterCount);
54+
55+
public:
56+
std::vector<uint8_t> laserCloudSurfKnnPassFlag;
57+
std::vector<uint8_t> laserCloudSurfPlaneValidFlag;
58+
std::vector<uint8_t> laserCloudSurfDebugCode;
59+
60+
static constexpr uint8_t SURF_DEBUG_ACCEPTED = 0;
61+
static constexpr uint8_t SURF_DEBUG_REJECTED_NEIGHBOR_COUNT = 1;
62+
static constexpr uint8_t SURF_DEBUG_REJECTED_KNN_DISTANCE = 2;
63+
static constexpr uint8_t SURF_DEBUG_REJECTED_PLANE_INVALID = 3;
64+
static constexpr uint8_t SURF_DEBUG_REJECTED_LOW_WEIGHT = 4;
65+
static constexpr uint8_t SURF_DEBUG_NOT_OPTIMIZED = 5;
66+
67+
ScanAligner(int max_points, float knn_distance, int cores);
68+
void setMap(const pcl::PointCloud<PointType>::Ptr &map, const pcl::KdTreeFLANN<PointType>::Ptr &kdtree);
69+
AlignmentMetrics align(const pcl::PointCloud<PointType>::Ptr &scan, float transformIn[6]);
70+
pcl::PointCloud<PointType>::Ptr getLaserCloudOri() const { return laserCloudOri; }
71+
};

‎src/mapOptimization/mapOptimization_core.cpp‎

Lines changed: 3 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -165,23 +165,10 @@ void mapOptimization::allocateMemory()
165165
kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
166166
kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
167167

168-
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
169-
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
168+
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());
169+
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>());
170170

171-
laserCloudOri.reset(new pcl::PointCloud<PointType>());
172-
coeffSel.reset(new pcl::PointCloud<PointType>());
173-
174-
laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
175-
coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
176-
laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
177-
laserCloudSurfKnnPassFlag.resize(N_SCAN * Horizon_SCAN);
178-
laserCloudSurfPlaneValidFlag.resize(N_SCAN * Horizon_SCAN);
179-
laserCloudSurfDebugCode.resize(N_SCAN * Horizon_SCAN);
180-
181-
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
182-
std::fill(laserCloudSurfKnnPassFlag.begin(), laserCloudSurfKnnPassFlag.end(), 0);
183-
std::fill(laserCloudSurfPlaneValidFlag.begin(), laserCloudSurfPlaneValidFlag.end(), 0);
184-
std::fill(laserCloudSurfDebugCode.begin(), laserCloudSurfDebugCode.end(), SURF_DEBUG_NOT_OPTIMIZED);
171+
scanAligner = std::make_shared<ScanAligner>(N_SCAN * Horizon_SCAN, surfKnnMinDistance, numberOfCores);
185172

186173
laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
187174
laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
@@ -192,8 +179,6 @@ void mapOptimization::allocateMemory()
192179
transformTobeMapped[i] = 0;
193180
}
194181

195-
matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
196-
197182
lastIncrementalDeltaPoseLocal = Eigen::Affine3f::Identity();
198183
hasLastIncrementalDeltaPoseLocal = false;
199184
}

‎src/mapOptimization/mapOptimization_publish.cpp‎

Lines changed: 11 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,5 @@
11
#include "mapOptimization/mapOptimization.hpp"
22

3-
void mapOptimization::pointAssociateToMap(PointType const * const pi, PointType * const po)
4-
{
5-
po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);
6-
po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);
7-
po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);
8-
po->intensity = pi->intensity;
9-
}
103

114
pcl::PointCloud<PointType>::Ptr mapOptimization::transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)
125
{
@@ -114,7 +107,8 @@ Eigen::Affine3f mapOptimization::affineFromTf(const tf2::Transform &transform) c
114107
void mapOptimization::updatePath(const PointTypePose& pose_in)
115108
{
116109
geometry_msgs::msg::PoseStamped pose_stamped;
117-
rclcpp::Time t(static_cast<uint32_t>(pose_in.time * 1e9));
110+
rclcpp::Time t(static_cast<int64_t>(pose_in.time * 1e9));
111+
118112
pose_stamped.header.stamp = t;
119113
pose_stamped.header.frame_id = mapFrameLocal;
120114
pose_stamped.pose.position.x = pose_in.x;
@@ -403,9 +397,9 @@ void mapOptimization::publishFrames()
403397
pcl::PointCloud<PointType>::Ptr transformedInput = transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
404398
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
405399

406-
const int codeBound = laserCloudSurfLastDSNum < static_cast<int>(laserCloudSurfDebugCode.size())
400+
const int codeBound = laserCloudSurfLastDSNum < static_cast<int>(scanAligner->laserCloudSurfDebugCode.size())
407401
? laserCloudSurfLastDSNum
408-
: static_cast<int>(laserCloudSurfDebugCode.size());
402+
: static_cast<int>(scanAligner->laserCloudSurfDebugCode.size());
409403
const int pointBound = codeBound < static_cast<int>(transformedInput->size())
410404
? codeBound
411405
: static_cast<int>(transformedInput->size());
@@ -418,21 +412,21 @@ void mapOptimization::publishFrames()
418412
point.y = transformedInput->points[i].y;
419413
point.z = transformedInput->points[i].z;
420414

421-
switch (laserCloudSurfDebugCode[i])
415+
switch (scanAligner->laserCloudSurfDebugCode[i])
422416
{
423-
case SURF_DEBUG_ACCEPTED:
417+
case ScanAligner::SURF_DEBUG_ACCEPTED:
424418
point.r = 0; point.g = 255; point.b = 0; // green
425419
break;
426-
case SURF_DEBUG_REJECTED_NEIGHBOR_COUNT:
420+
case ScanAligner::SURF_DEBUG_REJECTED_NEIGHBOR_COUNT:
427421
point.r = 255; point.g = 0; point.b = 0; // red
428422
break;
429-
case SURF_DEBUG_REJECTED_KNN_DISTANCE:
423+
case ScanAligner::SURF_DEBUG_REJECTED_KNN_DISTANCE:
430424
point.r = 255; point.g = 165; point.b = 0; // orange
431425
break;
432-
case SURF_DEBUG_REJECTED_PLANE_INVALID:
426+
case ScanAligner::SURF_DEBUG_REJECTED_PLANE_INVALID:
433427
point.r = 255; point.g = 255; point.b = 0; // yellow
434428
break;
435-
case SURF_DEBUG_REJECTED_LOW_WEIGHT:
429+
case ScanAligner::SURF_DEBUG_REJECTED_LOW_WEIGHT:
436430
point.r = 255; point.g = 0; point.b = 255; // magenta
437431
break;
438432
default:
@@ -473,7 +467,7 @@ void mapOptimization::publishFrames()
473467
if (pubMatchedSurfFeatures->get_subscription_count() != 0)
474468
{
475469
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
476-
*cloudOut += *transformPointCloud(laserCloudOri, &thisPose6D);
470+
*cloudOut += *transformPointCloud(scanAligner->getLaserCloudOri(), &thisPose6D);
477471
publishCloud(pubMatchedSurfFeatures, cloudOut, timeLaserInfoStamp, mapFrameLocal);
478472
}
479473
// publish registered high-res raw cloud
@@ -492,24 +486,6 @@ void mapOptimization::publishFrames()
492486
globalPath.header.frame_id = mapFrameLocal;
493487
pubPath->publish(globalPath);
494488
}
495-
// publish SLAM infomation for 3rd-party usage
496-
if (pubSLAMInfo->get_subscription_count() != 0)
497-
{
498-
// if (lastSLAMInfoPubSize != cloudKeyPoses6D->size())
499-
// {
500-
// liorf::msg::CloudInfo slamInfo;
501-
// slamInfo.header.stamp = timeLaserInfoStamp;
502-
// pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
503-
// *cloudOut += *laserCloudSurfLastDS;
504-
// slamInfo.key_frame_cloud = publishCloud(rclcpp::Publisher(), cloudOut, timeLaserInfoStamp, lidarFrame);
505-
// slamInfo.key_frame_poses = publishCloud(rclcpp::Publisher(), cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);
506-
// pcl::PointCloud<PointType>::Ptr localMapOut(new pcl::PointCloud<PointType>());
507-
// *localMapOut += *laserCloudSurfFromMapDS;
508-
// slamInfo.key_frame_map = publishCloud(rclcpp::Publisher(), localMapOut, timeLaserInfoStamp, odometryFrame);
509-
// pubSLAMInfo->publish(slamInfo);
510-
// lastSLAMInfoPubSize = cloudKeyPoses6D->size();
511-
// }
512-
}
513489
}
514490

515491

0 commit comments

Comments
 (0)