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}
)
# link_directories(${PCL_LIBRARY_DIRS})
# add_definitions(${PCL_DEFINITIONS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
include_directories(
src
@ -51,15 +51,15 @@ add_executable(main main.cc)
target_link_libraries(main
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${CERES_LIBRARIES}
${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES}
g3log
common
oh_my_loam
extractor
visualizer
helper
solver
odometry
oh_my_loam
solver
${CERES_LIBRARIES}
helper
)

1
configs/config.yaml
View File

@ -19,3 +19,4 @@ extractor_config:
odometry_config:
icp_iter_num : 2
dist_sq_thresh: 100

19
src/odometry/odometry.cc
View File

@ -5,7 +5,6 @@ namespace oh_my_loam {
namespace {
int kNearbyScanNum = 2;
double kDistSquareThresh = 25;
size_t kMinMatchNum = 10;
} // namespace
@ -23,15 +22,17 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
*pose = pose_curr2world_;
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) {
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
AssociateCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
kDistSquareThresh);
AssociateSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
kDistSquareThresh);
MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
match_dist_sq_thresh);
MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
match_dist_sq_thresh);
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* p = pose_curr2last_.p().data();
@ -50,8 +51,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
UpdatePre(feature);
}
void Odometry::AssociateCornPoints(const TPointCloud& src,
const TPointCloud& tgt,
void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_thresh) const {
kdtree_corn_pts_->setInputCloud(tgt.makeShared());
@ -93,8 +93,7 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
}
}
void Odometry::AssociateSurfPoints(const TPointCloud& src,
const TPointCloud& tgt,
void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_thresh) const {
kdtree_surf_pts_->setInputCloud(tgt.makeShared());

4
src/odometry/odometry.h
View File

@ -18,11 +18,11 @@ class Odometry {
protected:
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,
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,
double dist_thresh) const;