Fix ICP sample indices after initialization

[keenan-burnett]

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.