Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: add score array #55

Merged
merged 5 commits into from
Jun 2, 2024
Merged

feat: add score array #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
63cb6b1
Added score_array
SakodaShintaro Mar 9, 2024
9d8135a
Merge branch 'tier4/main' into feat/add_score_array
SakodaShintaro Mar 11, 2024
3edc679
Merge remote-tracking branch 'origin' into feat/add_score_array
SakodaShintaro May 31, 2024
164607d
Fixed transform_probability
SakodaShintaro May 31, 2024
db9a9d9
Added print gains
SakodaShintaro May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion apps/regression_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,14 +107,17 @@ int main(int argc, char** argv) {
const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
const double elapsed = timer.elapsed_milliseconds();

const float gain_tp = ndt_result.transform_probability - ndt_result.transform_probability_array.front();
const float gain_nvtl = ndt_result.nearest_voxel_transformation_likelihood - ndt_result.nearest_voxel_transformation_likelihood_array.front();

// output result
const double tp = ndt_result.transform_probability;
const double nvtl = ndt_result.nearest_voxel_transformation_likelihood;
elapsed_milliseconds.push_back(elapsed);
nvtl_scores.push_back(nvtl);
tp_scores.push_back(tp);
if(i % update_interval == 0) {
std::cout << "source_cloud->size()=" << std::setw(4) << source_cloud->size() << ", time=" << elapsed << " [msec], nvtl=" << nvtl << ", tp = " << tp << std::endl;
std::cout << "source_cloud->size()=" << std::setw(4) << source_cloud->size() << ", time=" << elapsed << " [msec], nvtl=" << nvtl << ", tp = " << tp << ", gain_nvtl=" << gain_nvtl << ", gain_tp=" << gain_tp << std::endl;
}
}

Expand Down
4 changes: 4 additions & 0 deletions include/multigrid_pclomp/multigrid_ndt_omp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -278,7 +278,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
ndt_result.pose = this->getFinalTransformation();
ndt_result.transformation_array = getFinalTransformationArray();
ndt_result.transform_probability = getTransformationProbability();
ndt_result.transform_probability_array = transform_probability_array_;
ndt_result.nearest_voxel_transformation_likelihood = getNearestVoxelTransformationLikelihood();
ndt_result.nearest_voxel_transformation_likelihood_array = nearest_voxel_transformation_likelihood_array_;
ndt_result.iteration_num = getFinalNumIteration();
ndt_result.hessian = getHessian();
return ndt_result;
Expand Down Expand Up @@ -515,6 +517,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour

Eigen::Matrix<double, 6, 6> hessian_;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array_;
std::vector<float> transform_probability_array_;
std::vector<float> nearest_voxel_transformation_likelihood_array_;
double nearest_voxel_transformation_likelihood_;

boost::optional<Eigen::Matrix4f> regularization_pose_;
Expand Down
15 changes: 14 additions & 1 deletion include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormal

hessian_ = other.hessian_;
transformation_array_ = other.transformation_array_;
transform_probability_array_ = other.transform_probability_array_;
nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;

regularization_pose_ = other.regularization_pose_;
Expand All @@ -88,6 +90,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormal

hessian_ = other.hessian_;
transformation_array_ = other.transformation_array_;
transform_probability_array_ = other.transform_probability_array_;
nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;

regularization_pose_ = other.regularization_pose_;
Expand All @@ -109,6 +113,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget> &MultiGridNormal

hessian_ = other.hessian_;
transformation_array_ = other.transformation_array_;
transform_probability_array_ = other.transform_probability_array_;
nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;

regularization_pose_ = other.regularization_pose_;
Expand All @@ -132,6 +138,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget> &MultiGridNormal

hessian_ = other.hessian_;
transformation_array_ = other.transformation_array_;
transform_probability_array_ = other.transform_probability_array_;
nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;

regularization_pose_ = other.regularization_pose_;
Expand Down Expand Up @@ -192,6 +200,9 @@ void MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeTra
transformation_array_.clear();
transformation_array_.push_back(final_transformation_);

transform_probability_array_.clear();
nearest_voxel_transformation_likelihood_array_.clear();

// Convert initial guess matrix to 6 element transformation vector
Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
Eigen::Vector3f init_translation = eig_transformation.translation();
Expand Down Expand Up @@ -409,7 +420,9 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeD
} else {
nearest_voxel_transformation_likelihood_ = 0.0;
}

nearest_voxel_transformation_likelihood_array_.push_back(nearest_voxel_transformation_likelihood_);
const float transform_probability = (input_->points.empty() ? 0.0f : score / static_cast<double>(input_->points.size()));
transform_probability_array_.push_back(transform_probability);
return (score);
}

Expand Down
2 changes: 2 additions & 0 deletions include/pclomp/ndt_struct.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ struct NdtResult {
int iteration_num;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
Eigen::Matrix<double, 6, 6> hessian;
std::vector<float> transform_probability_array;
std::vector<float> nearest_voxel_transformation_likelihood_array;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {
Expand Down
Loading