Skip to content
Snippets Groups Projects
Commit 5567d225 authored by Víctor Sainz Ubide's avatar Víctor Sainz Ubide
Browse files

Merge branch '27-new-branch-laser-3d-2' of...

Merge branch '27-new-branch-laser-3d-2' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser into 27-new-branch-laser-3d-2
parents 8154a235 ec94c1db
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
......@@ -51,6 +51,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
{
double max_time_span;
double max_fitness_score; // maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
bool pcl_downsample;
double pcl_downsample_voxel_size;
......@@ -68,6 +69,7 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
: ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
{
max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
max_fitness_score = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_fitness_score");
pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size");
......
......@@ -101,17 +101,9 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
auto start = std::chrono::high_resolution_clock::now();
// DURATION --------------------------------------------------------
// Internal PointCloud pointers (boost::shared_ptr)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
// Point clouds are considered already downsampled
*cloud_ref = *_cloud_ref;
*cloud_other = *_cloud_other;
// Set input clouds
_registration_solver->setInputSource(cloud_ref);
_registration_solver->setInputTarget(cloud_other);
_registration_solver->setInputSource(_cloud_ref);
_registration_solver->setInputTarget(_cloud_other);
// Declare variables
Eigen::Matrix4f transform_pcl;
......@@ -120,7 +112,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>();
// Alignment
_registration_solver->align(*cloud_ref, transform_guess);
pcl::PointCloud<pcl::PointXYZ>cloud_out;
_registration_solver->align(cloud_out, transform_guess);
transform_pcl = _registration_solver->getFinalTransformation();
// Output the transformation as Isometry of double(s)
......
......@@ -179,14 +179,24 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
*/
bool ProcessorOdomIcp3d::voteForKeyFrame() const
{
// 1. vote by time
if (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_odom_icp_->max_time_span)
{
WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by time");
return true;
}
// TODO:
// - vote by distance
// - vote by angle
// - vote by nbr. of captures
// 2. vote by distance
// 3. vote by angle
// 4. vote by nbr. of captures
// 5. vote by fitness score
if (registration_solver_->getFitnessScore() > params_odom_icp_->max_fitness_score)
{
WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by fitness score");
return true;
}
return false;
};
......
......@@ -49,6 +49,8 @@ using namespace wolf;
std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
class Test_ProcessorOdomIcp3D_yaml : public testing::Test
{
public:
......
......@@ -37,6 +37,7 @@ config:
voting_active: true
min_features_for_keyframe : 0
max_time_span: 0.99
max_fitness_score: 0.0001 # maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
angle_turned: 1.0
state_getter: true
state_priority: 1
......@@ -44,7 +45,7 @@ config:
pcl_downsample_voxel_size: 0.05
icp_algorithm: "icp_nl" # "icp", "icp_nl", "gicp"
icp_max_iterations: 20
icp_transformation_rotation_epsilon: 0.99 # this is cos(alpha)
icp_transformation_rotation_epsilon: 0.9999 # this is cos(alpha)
icp_transformation_translation_epsilon: 1e-9 # this is translation squared
icp_max_correspondence_distance: 0.05
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment