Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Dec 9, 2021
1 parent afcaefe commit ba21fcd
Show file tree
Hide file tree
Showing 8 changed files with 21 additions and 14 deletions.
3 changes: 2 additions & 1 deletion localization/ndt/include/ndt/base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ class NormalDistributionsTransformBase
virtual boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const = 0;
virtual boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const = 0;
virtual Eigen::Matrix4f getFinalTransformation() const = 0;
virtual std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getFinalTransformationArray() const = 0;
virtual std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
getFinalTransformationArray() const = 0;

virtual Eigen::Matrix<double, 6, 6> getHessian() const = 0;

Expand Down
5 changes: 3 additions & 2 deletions localization/ndt/include/ndt/impl/pcl_generic.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -138,8 +138,9 @@ NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFinalTransf
}

template <class PointSource, class PointTarget>
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> NormalDistributionsTransformPCLGeneric<
PointSource, PointTarget>::getFinalTransformationArray() const
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFinalTransformationArray()
const
{
return std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>();
}
Expand Down
5 changes: 3 additions & 2 deletions localization/ndt/include/ndt/impl/pcl_modified.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -139,8 +139,9 @@ NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFinalTrans
}

template <class PointSource, class PointTarget>
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> NormalDistributionsTransformPCLModified<
PointSource, PointTarget>::getFinalTransformationArray() const
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFinalTransformationArray()
const
{
return ndt_ptr_->getFinalTransformationArray();
}
Expand Down
3 changes: 2 additions & 1 deletion localization/ndt/include/ndt/omp.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ class NormalDistributionsTransformOMP
boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
Eigen::Matrix4f getFinalTransformation() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getFinalTransformationArray() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
getFinalTransformationArray() const override;

Eigen::Matrix<double, 6, 6> getHessian() const override;

Expand Down
3 changes: 2 additions & 1 deletion localization/ndt/include/ndt/pcl_generic.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ class NormalDistributionsTransformPCLGeneric
boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
Eigen::Matrix4f getFinalTransformation() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getFinalTransformationArray() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
getFinalTransformationArray() const override;

Eigen::Matrix<double, 6, 6> getHessian() const override;

Expand Down
3 changes: 2 additions & 1 deletion localization/ndt/include/ndt/pcl_modified.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class NormalDistributionsTransformPCLModified
boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
Eigen::Matrix4f getFinalTransformation() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getFinalTransformationArray() const override;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
getFinalTransformationArray() const override;

Eigen::Matrix<double, 6, 6> getHessian() const override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,8 @@ class NormalDistributionsTransformModified

inline const Eigen::Matrix<double, 6, 6> getHessian() const { return hessian_; }

inline const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getFinalTransformationArray() const
inline const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
getFinalTransformationArray() const
{
return transformation_array_;
}
Expand Down
10 changes: 5 additions & 5 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,9 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin
}

bool isLocalOptimalSolutionOscillation(
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> & result_pose_matrix_array,
const float oscillation_threshold,
const float inversion_vector_threshold)
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &
result_pose_matrix_array,
const float oscillation_threshold, const float inversion_vector_threshold)
{
bool prev_oscillation = false;
int oscillation_cnt = 0;
Expand Down Expand Up @@ -433,8 +433,8 @@ void NDTScanMatcher::callbackSensorPoints(
result_pose_affine.matrix() = result_pose_matrix.cast<double>();
const geometry_msgs::msg::Pose result_pose_msg = tf2::toMsg(result_pose_affine);

const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> result_pose_matrix_array =
ndt_ptr_->getFinalTransformationArray();
const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
result_pose_matrix_array = ndt_ptr_->getFinalTransformationArray();
std::vector<geometry_msgs::msg::Pose> result_pose_msg_array;
for (const auto & pose_matrix : result_pose_matrix_array) {
Eigen::Affine3d pose_affine;
Expand Down

0 comments on commit ba21fcd

Please sign in to comment.