Skip to content

Commit dfcdc9d

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

7 files changed

Lines changed: 436 additions & 404 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<bool> 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: 9 additions & 34 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
{
@@ -403,9 +396,9 @@ void mapOptimization::publishFrames()
403396
pcl::PointCloud<PointType>::Ptr transformedInput = transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
404397
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
405398

406-
const int codeBound = laserCloudSurfLastDSNum < static_cast<int>(laserCloudSurfDebugCode.size())
399+
const int codeBound = laserCloudSurfLastDSNum < static_cast<int>(scanAligner->laserCloudSurfDebugCode.size())
407400
? laserCloudSurfLastDSNum
408-
: static_cast<int>(laserCloudSurfDebugCode.size());
401+
: static_cast<int>(scanAligner->laserCloudSurfDebugCode.size());
409402
const int pointBound = codeBound < static_cast<int>(transformedInput->size())
410403
? codeBound
411404
: static_cast<int>(transformedInput->size());
@@ -418,21 +411,21 @@ void mapOptimization::publishFrames()
418411
point.y = transformedInput->points[i].y;
419412
point.z = transformedInput->points[i].z;
420413

421-
switch (laserCloudSurfDebugCode[i])
414+
switch (scanAligner->laserCloudSurfDebugCode[i])
422415
{
423-
case SURF_DEBUG_ACCEPTED:
416+
case ScanAligner::SURF_DEBUG_ACCEPTED:
424417
point.r = 0; point.g = 255; point.b = 0; // green
425418
break;
426-
case SURF_DEBUG_REJECTED_NEIGHBOR_COUNT:
419+
case ScanAligner::SURF_DEBUG_REJECTED_NEIGHBOR_COUNT:
427420
point.r = 255; point.g = 0; point.b = 0; // red
428421
break;
429-
case SURF_DEBUG_REJECTED_KNN_DISTANCE:
422+
case ScanAligner::SURF_DEBUG_REJECTED_KNN_DISTANCE:
430423
point.r = 255; point.g = 165; point.b = 0; // orange
431424
break;
432-
case SURF_DEBUG_REJECTED_PLANE_INVALID:
425+
case ScanAligner::SURF_DEBUG_REJECTED_PLANE_INVALID:
433426
point.r = 255; point.g = 255; point.b = 0; // yellow
434427
break;
435-
case SURF_DEBUG_REJECTED_LOW_WEIGHT:
428+
case ScanAligner::SURF_DEBUG_REJECTED_LOW_WEIGHT:
436429
point.r = 255; point.g = 0; point.b = 255; // magenta
437430
break;
438431
default:
@@ -473,7 +466,7 @@ void mapOptimization::publishFrames()
473466
if (pubMatchedSurfFeatures->get_subscription_count() != 0)
474467
{
475468
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
476-
*cloudOut += *transformPointCloud(laserCloudOri, &thisPose6D);
469+
*cloudOut += *transformPointCloud(scanAligner->getLaserCloudOri(), &thisPose6D);
477470
publishCloud(pubMatchedSurfFeatures, cloudOut, timeLaserInfoStamp, mapFrameLocal);
478471
}
479472
// publish registered high-res raw cloud
@@ -492,24 +485,6 @@ void mapOptimization::publishFrames()
492485
globalPath.header.frame_id = mapFrameLocal;
493486
pubPath->publish(globalPath);
494487
}
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-
}
513488
}
514489

515490

0 commit comments

Comments
 (0)