diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index caf15cf..07ea8d0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -68,7 +68,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id, imu_data_.resize( std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( - mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); + sensor::ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 3704774..47aaf68 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -28,7 +28,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" -#include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/sensor/imu_data.h" namespace cartographer { namespace mapping_2d { @@ -86,7 +86,7 @@ class OptimizationProblem { int num_trimmed_submaps = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; - std::vector> imu_data_; + std::vector> imu_data_; std::vector> node_data_; std::vector> submap_data_; std::vector trajectory_data_; diff --git a/cartographer/mapping_3d/imu_integration.cc b/cartographer/mapping_3d/imu_integration.cc index 542e7da..827e8ee 100644 --- a/cartographer/mapping_3d/imu_integration.cc +++ b/cartographer/mapping_3d/imu_integration.cc @@ -20,8 +20,9 @@ namespace cartographer { namespace mapping_3d { IntegrateImuResult IntegrateImu( - const std::deque& imu_data, const common::Time start_time, - const common::Time end_time, std::deque::const_iterator* it) { + const std::deque& imu_data, const common::Time start_time, + const common::Time end_time, + std::deque::const_iterator* it) { return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(), start_time, end_time, it); diff --git a/cartographer/mapping_3d/imu_integration.h b/cartographer/mapping_3d/imu_integration.h index 1c63767..f3125ad 100644 --- a/cartographer/mapping_3d/imu_integration.h +++ b/cartographer/mapping_3d/imu_integration.h @@ -23,18 +23,13 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/time.h" +#include "cartographer/sensor/imu_data.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { -struct ImuData { - common::Time time; - Eigen::Vector3d linear_acceleration; - Eigen::Vector3d angular_velocity; -}; - template struct IntegrateImuResult { Eigen::Matrix delta_velocity; @@ -43,17 +38,18 @@ struct IntegrateImuResult { // Returns velocity delta in map frame. IntegrateImuResult IntegrateImu( - const std::deque& imu_data, const common::Time start_time, - const common::Time end_time, std::deque::const_iterator* it); + const std::deque& imu_data, const common::Time start_time, + const common::Time end_time, + std::deque::const_iterator* it); template IntegrateImuResult IntegrateImu( - const std::deque& imu_data, + const std::deque& imu_data, const Eigen::Transform& linear_acceleration_calibration, const Eigen::Transform& angular_velocity_calibration, const common::Time start_time, const common::Time end_time, - std::deque::const_iterator* it) { + std::deque::const_iterator* it) { CHECK_LE(start_time, end_time); CHECK(*it != imu_data.cend()); CHECK_LE((*it)->time, start_time); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 2a6864a..efbce1b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -31,6 +31,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping_3d/acceleration_cost_function.h" #include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" @@ -86,7 +87,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id, imu_data_.resize( std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( - ImuData{time, linear_acceleration, angular_velocity}); + sensor::ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( @@ -197,7 +198,7 @@ void OptimizationProblem::Solve(const std::vector& constraints) { TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4, new ceres::QuaternionParameterization()); - const std::deque& imu_data = imu_data_.at(trajectory_id); + const std::deque& imu_data = imu_data_.at(trajectory_id); CHECK(!imu_data.empty()); // TODO(whess): Add support for empty trajectories. const auto& node_data = node_data_[trajectory_id]; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 9f1ce3b..355be78 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -28,7 +28,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" -#include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/sensor/imu_data.h" namespace cartographer { namespace mapping_3d { @@ -82,7 +82,7 @@ class OptimizationProblem { mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; - std::vector> imu_data_; + std::vector> imu_data_; std::vector> node_data_; std::vector> submap_data_; std::vector trajectory_data_; diff --git a/cartographer/sensor/imu_data.cc b/cartographer/sensor/imu_data.cc new file mode 100644 index 0000000..61d6cab --- /dev/null +++ b/cartographer/sensor/imu_data.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/sensor/imu_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::ImuData ToProto(const ImuData& imu_data) { + proto::ImuData proto; + proto.set_timestamp(common::ToUniversal(imu_data.time)); + *proto.mutable_linear_acceleration() = + transform::ToProto(imu_data.linear_acceleration); + *proto.mutable_angular_velocity() = + transform::ToProto(imu_data.angular_velocity); + return proto; +} + +ImuData FromProto(const proto::ImuData& proto) { + return ImuData{common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.linear_acceleration()), + transform::ToEigen(proto.angular_velocity())}; +} + +} // sensor +} // cartographer diff --git a/cartographer/sensor/imu_data.h b/cartographer/sensor/imu_data.h new file mode 100644 index 0000000..18f1b6e --- /dev/null +++ b/cartographer/sensor/imu_data.h @@ -0,0 +1,42 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_IMU_DATA_H_ +#define CARTOGRAPHER_SENSOR_IMU_DATA_H_ + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +struct ImuData { + common::Time time; + Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; +}; + +// Converts 'imu_data' to a proto::ImuData. +proto::ImuData ToProto(const ImuData& imu_data); + +// Converts 'proto' to an ImuData. +ImuData FromProto(const proto::ImuData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_IMU_DATA_H_ diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 171b001..ee59637 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -40,3 +40,10 @@ message RangeData { optional PointCloud point_cloud = 2; optional PointCloud missing_echo_point_cloud = 3; } + +// Proto representation of ::cartographer::sensor::ImuData +message ImuData { + optional int64 timestamp = 1; + optional transform.proto.Vector3d linear_acceleration = 2; + optional transform.proto.Vector3d angular_velocity = 3; +}