Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Fix ICP sample indices after initialization #183

New issue
New issue
Open
Open
Fix ICP sample indices after initialization#183
Assignees
keenan-burnett
Labels
enhancementNew feature or request
@keenan-burnett

Description

@keenan-burnett
keenan-burnett
opened on May 11, 2023

Currently, we resample the filtered_samples_inds at each iteration of ICP:

vtr3/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp

Lines 281 to 295 in 0dc3213

for (size_t i = 0; i < sample_inds.size(); i++) {
if (nn_dists[i] < max_pair_d2) {
// Check planar distance (only after a few steps for initial alignment)
auto diff = aligned_points[sample_inds[i].first].getVector3fMap() -
point_map[sample_inds[i].second].getVector3fMap();
float planar_dist = std::abs(
diff.dot(point_map[sample_inds[i].second].getNormalVector3fMap()));
if (step < first_steps || planar_dist < max_planar_d) {
filtered_sample_inds.push_back(sample_inds[i]);
}
}
}
timer[2]->stop();
/// point to plane optimization

This essentially "moves" the minimum of the cost function at each iteration of ICP.
If we fixed filtered_samples_inds after the initialization steps, it might allow ICP to converge faster.

Currently, it often takes 20-40 iterations of ICP to converge which seems like too much.

Metadata

Metadata

Assignees

Labels

enhancementNew feature or request

Type

No type

Projects

No projects

Milestone

No milestone

Relationships

None yet

Development

No branches or pull requests

Issue actions