diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 815739b..5a40c9b 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -680,7 +680,7 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id, transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( const int trajectory_id, const common::Time time) const { - CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); + CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0); const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 503a7a7..18219a1 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -93,9 +93,9 @@ class PoseGraph2D : public PoseGraph { EXCLUDES(mutex_); void FinishTrajectory(int trajectory_id) override; - bool IsTrajectoryFinished(int trajectory_id) override; + bool IsTrajectoryFinished(int trajectory_id) override REQUIRES(mutex_); void FreezeTrajectory(int trajectory_id) override; - bool IsTrajectoryFrozen(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) override REQUIRES(mutex_); void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, @@ -284,7 +284,7 @@ class PoseGraph2D : public PoseGraph { std::vector GetConstraints() const override; void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; - bool IsFinished(int trajectory_id) const override; + bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex); private: PoseGraph2D* const parent_; diff --git a/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h b/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h index 6edf469..4c1e25a 100644 --- a/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ #include "Eigen/Core" +#include "ceres/ceres.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h b/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h index e0c3d62..b92f7c7 100644 --- a/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ #include "Eigen/Core" +#include "ceres/ceres.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h b/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h index 678a18d..b90293d 100644 --- a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h @@ -19,6 +19,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "ceres/ceres.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 638a734..e2d1df7 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -713,7 +713,7 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id, transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose( const int trajectory_id, const common::Time time) const { - CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); + CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0); const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index f81d3fb..515e982 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -92,9 +92,9 @@ class PoseGraph3D : public PoseGraph { EXCLUDES(mutex_); void FinishTrajectory(int trajectory_id) override; - bool IsTrajectoryFinished(int trajectory_id) override; + bool IsTrajectoryFinished(int trajectory_id) override REQUIRES(mutex_); void FreezeTrajectory(int trajectory_id) override; - bool IsTrajectoryFrozen(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) override REQUIRES(mutex_); void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, @@ -289,7 +289,7 @@ class PoseGraph3D : public PoseGraph { std::vector GetConstraints() const override; void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; - bool IsFinished(int trajectory_id) const override; + bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); private: PoseGraph3D* const parent_; diff --git a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h b/cartographer/mapping/internal/3d/rotation_cost_function_3d.h index 1bd709e..cd61190 100644 --- a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/rotation_cost_function_3d.h @@ -19,6 +19,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "ceres/ceres.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h b/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h index 91464fa..7959eaf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ #include "Eigen/Core" +#include "ceres/ceres.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h index 94484d1..eee01cf 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h @@ -17,6 +17,10 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + namespace cartographer { namespace mapping { namespace pose_graph { diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 7deb8fc..9750fab 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -43,7 +43,8 @@ PoseGraph::Constraint::Tag FromProto( case proto::PoseGraph::Constraint::INTER_SUBMAP: return PoseGraph::Constraint::Tag::INTER_SUBMAP; case ::google::protobuf::kint32max: - case ::google::protobuf::kint32min:; + case ::google::protobuf::kint32min: { + } } LOG(FATAL) << "Unsupported tag."; }