12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

build ok

main
feixyz10 2020-10-29 21:12:43 +08:00 committed by feixyz
parent 9663294c81
commit 5e2a26e1ab
4 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -30,8 +30,8 @@ include_directories(SYSTEM
${G3LOG_INCLUDE_DIRS} ${G3LOG_INCLUDE_DIRS}
) )
# link_directories(${PCL_LIBRARY_DIRS}) link_directories(${PCL_LIBRARY_DIRS})
# add_definitions(${PCL_DEFINITIONS}) add_definitions(${PCL_DEFINITIONS})
include_directories( include_directories(
src src
@ -51,15 +51,15 @@ add_executable(main main.cc)
target_link_libraries(main target_link_libraries(main
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${PCL_LIBRARIES} ${PCL_LIBRARIES}
${CERES_LIBRARIES}
${G3LOG_LIBRARIES} ${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES} ${YAML_CPP_LIBRARIES}
g3log g3log
common common
oh_my_loam
extractor extractor
visualizer visualizer
helper
solver
odometry odometry
oh_my_loam solver
${CERES_LIBRARIES}
helper
) )

3
configs/config.yaml
View File

@ -18,4 +18,5 @@ extractor_config:
downsample_voxel_size: 0.2 downsample_voxel_size: 0.2
odometry_config: odometry_config:
icp_iter_num : 2 icp_iter_num : 2
dist_sq_thresh: 100

27
src/odometry/odometry.cc
View File

@ -5,7 +5,6 @@ namespace oh_my_loam {
namespace { namespace {
int kNearbyScanNum = 2; int kNearbyScanNum = 2;
double kDistSquareThresh = 25;
size_t kMinMatchNum = 10; size_t kMinMatchNum = 10;
} // namespace } // namespace
@ -23,15 +22,17 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
*pose = pose_curr2world_; *pose = pose_curr2world_;
return; return;
} }
double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
std::vector<PointLinePair> pl_pairs; std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
AssociateCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs, MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
kDistSquareThresh); match_dist_sq_thresh);
AssociateSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs, MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
kDistSquareThresh); match_dist_sq_thresh);
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) { if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence"; AWARN << "Too less correspondence: num = "
<< pl_pairs.size() + pp_pairs.size();
} }
double* q = pose_curr2last_.q().coeffs().data(); double* q = pose_curr2last_.q().coeffs().data();
double* p = pose_curr2last_.p().data(); double* p = pose_curr2last_.p().data();
@ -50,10 +51,9 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
UpdatePre(feature); UpdatePre(feature);
} }
void Odometry::AssociateCornPoints(const TPointCloud& src, void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
const TPointCloud& tgt, std::vector<PointLinePair>* const pairs,
std::vector<PointLinePair>* const pairs, double dist_thresh) const {
double dist_thresh) const {
kdtree_corn_pts_->setInputCloud(tgt.makeShared()); kdtree_corn_pts_->setInputCloud(tgt.makeShared());
for (const auto& query_pt : src) { for (const auto& query_pt : src) {
std::vector<int> indices; std::vector<int> indices;
@ -93,10 +93,9 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
} }
} }
void Odometry::AssociateSurfPoints(const TPointCloud& src, void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
const TPointCloud& tgt, std::vector<PointPlanePair>* const pairs,
std::vector<PointPlanePair>* const pairs, double dist_thresh) const {
double dist_thresh) const {
kdtree_surf_pts_->setInputCloud(tgt.makeShared()); kdtree_surf_pts_->setInputCloud(tgt.makeShared());
for (const auto& query_pt : src) { for (const auto& query_pt : src) {
std::vector<int> indices; std::vector<int> indices;

12
src/odometry/odometry.h
View File

@ -18,13 +18,13 @@ class Odometry {
protected: protected:
void UpdatePre(const FeaturePoints& feature); void UpdatePre(const FeaturePoints& feature);
void AssociateCornPoints(const TPointCloud& src, const TPointCloud& tgt, void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs, std::vector<PointLinePair>* const pairs,
double dist_thresh) const; double dist_thresh) const;
void AssociateSurfPoints(const TPointCloud& src, const TPointCloud& tgt, void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs, std::vector<PointPlanePair>* const pairs,
double dist_thresh) const; double dist_thresh) const;
Pose3D pose_curr2world_; Pose3D pose_curr2world_;
Pose3D pose_curr2last_; Pose3D pose_curr2last_;