12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move ImuData to sensor, add ImuData proto. (#381)

master
Jihoon Lee 2017-07-05 12:28:40 +02:00 committed by Wolfgang Hess
parent 9b48ba5ff4
commit 1e6723e214
9 changed files with 107 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -68,7 +68,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
imu_data_.resize(
std::max(imu_data_.size(), static_cast<size_t>(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(

4
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -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<std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::deque<NodeData>> node_data_;
std::vector<std::deque<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;

5
cartographer/mapping_3d/imu_integration.cc
View File

@ -20,8 +20,9 @@ namespace cartographer {
namespace mapping_3d {
IntegrateImuResult<double> IntegrateImu(
const std::deque<ImuData>& imu_data, const common::Time start_time,
const common::Time end_time, std::deque<ImuData>::const_iterator* it) {
const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
const common::Time end_time,
std::deque<sensor::ImuData>::const_iterator* it) {
return IntegrateImu<double>(imu_data, Eigen::Affine3d::Identity(),
Eigen::Affine3d::Identity(), start_time, end_time,
it);

16
cartographer/mapping_3d/imu_integration.h
View File

@ -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 <typename T>
struct IntegrateImuResult {
Eigen::Matrix<T, 3, 1> delta_velocity;
@ -43,17 +38,18 @@ struct IntegrateImuResult {
// Returns velocity delta in map frame.
IntegrateImuResult<double> IntegrateImu(
const std::deque<ImuData>& imu_data, const common::Time start_time,
const common::Time end_time, std::deque<ImuData>::const_iterator* it);
const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
const common::Time end_time,
std::deque<sensor::ImuData>::const_iterator* it);
template <typename T>
IntegrateImuResult<T> IntegrateImu(
const std::deque<ImuData>& imu_data,
const std::deque<sensor::ImuData>& imu_data,
const Eigen::Transform<T, 3, Eigen::Affine>&
linear_acceleration_calibration,
const Eigen::Transform<T, 3, Eigen::Affine>& angular_velocity_calibration,
const common::Time start_time, const common::Time end_time,
std::deque<ImuData>::const_iterator* it) {
std::deque<sensor::ImuData>::const_iterator* it) {
CHECK_LE(start_time, end_time);
CHECK(*it != imu_data.cend());
CHECK_LE((*it)->time, start_time);

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -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<size_t>(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<Constraint>& constraints) {
TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
new ceres::QuaternionParameterization());
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
const std::deque<sensor::ImuData>& 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];

4
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -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<std::deque<ImuData>> imu_data_;
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::vector<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;

41
cartographer/sensor/imu_data.cc Normal file
View File

@ -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

42
cartographer/sensor/imu_data.h Normal file
View File

@ -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_

7
cartographer/sensor/proto/sensor.proto
View File

@ -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;
}