Making pose in FixedFramePoseData optional. (#792)
This is for the case that the GPS signal is not available.master
parent
196b4b891c
commit
91034eaf58
|
@ -85,7 +85,10 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
|
||||||
void AddSensorData(
|
void AddSensorData(
|
||||||
const std::string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor::FixedFramePoseData& fixed_frame_pose) override {
|
const sensor::FixedFramePoseData& fixed_frame_pose) override {
|
||||||
CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose;
|
if (fixed_frame_pose.pose.has_value()) {
|
||||||
|
CHECK(fixed_frame_pose.pose.value().IsValid())
|
||||||
|
<< fixed_frame_pose.pose.value();
|
||||||
|
}
|
||||||
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
|
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -49,11 +49,10 @@ namespace pose_graph {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
// For odometry and fixed frame pose.
|
// For odometry.
|
||||||
template <typename MapByTimeType>
|
|
||||||
std::unique_ptr<transform::Rigid3d> Interpolate(
|
std::unique_ptr<transform::Rigid3d> Interpolate(
|
||||||
const MapByTimeType& map_by_time, const int trajectory_id,
|
const sensor::MapByTime<sensor::OdometryData>& map_by_time,
|
||||||
const common::Time time) {
|
const int trajectory_id, const common::Time time) {
|
||||||
const auto it = map_by_time.lower_bound(trajectory_id, time);
|
const auto it = map_by_time.lower_bound(trajectory_id, time);
|
||||||
if (it == map_by_time.EndOfTrajectory(trajectory_id)) {
|
if (it == map_by_time.EndOfTrajectory(trajectory_id)) {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
|
@ -71,6 +70,33 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
|
||||||
.transform);
|
.transform);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// For fixed frame pose.
|
||||||
|
std::unique_ptr<transform::Rigid3d> Interpolate(
|
||||||
|
const sensor::MapByTime<sensor::FixedFramePoseData>& map_by_time,
|
||||||
|
const int trajectory_id, const common::Time time) {
|
||||||
|
const auto it = map_by_time.lower_bound(trajectory_id, time);
|
||||||
|
if (it == map_by_time.EndOfTrajectory(trajectory_id) ||
|
||||||
|
!it->pose.has_value()) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
|
||||||
|
if (it->time == time) {
|
||||||
|
return common::make_unique<transform::Rigid3d>(it->pose.value());
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
const auto prev_it = std::prev(it);
|
||||||
|
if (prev_it->pose.has_value()) {
|
||||||
|
return common::make_unique<transform::Rigid3d>(
|
||||||
|
Interpolate(transform::TimestampedTransform{prev_it->time,
|
||||||
|
prev_it->pose.value()},
|
||||||
|
transform::TimestampedTransform{it->time, it->pose.value()},
|
||||||
|
time)
|
||||||
|
.transform);
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
OptimizationProblem::OptimizationProblem(
|
OptimizationProblem::OptimizationProblem(
|
||||||
|
|
|
@ -16,6 +16,7 @@
|
||||||
|
|
||||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||||
|
|
||||||
|
#include "cartographer/common/optional.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
@ -24,13 +25,18 @@ namespace sensor {
|
||||||
proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) {
|
proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) {
|
||||||
proto::FixedFramePoseData proto;
|
proto::FixedFramePoseData proto;
|
||||||
proto.set_timestamp(common::ToUniversal(pose_data.time));
|
proto.set_timestamp(common::ToUniversal(pose_data.time));
|
||||||
*proto.mutable_pose() = transform::ToProto(pose_data.pose);
|
if (pose_data.pose.has_value()) {
|
||||||
|
*proto.mutable_pose() = transform::ToProto(pose_data.pose.value());
|
||||||
|
}
|
||||||
return proto;
|
return proto;
|
||||||
}
|
}
|
||||||
|
|
||||||
FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) {
|
FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) {
|
||||||
return FixedFramePoseData{common::FromUniversal(proto.timestamp()),
|
return FixedFramePoseData{common::FromUniversal(proto.timestamp()),
|
||||||
transform::ToRigid3(proto.pose())};
|
proto.has_pose()
|
||||||
|
? common::optional<transform::Rigid3d>(
|
||||||
|
transform::ToRigid3(proto.pose()))
|
||||||
|
: common::optional<transform::Rigid3d>()};
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
|
|
|
@ -17,6 +17,9 @@
|
||||||
#ifndef CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
|
#ifndef CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
|
||||||
#define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
|
#define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
#include "cartographer/common/optional.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
@ -24,11 +27,11 @@
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace sensor {
|
namespace sensor {
|
||||||
|
|
||||||
// The fixed frame pose data(like gps, pose, etc.) will be used in the
|
// The fixed frame pose data (like GPS, pose, etc.) will be used in the
|
||||||
// optimization.
|
// optimization.
|
||||||
struct FixedFramePoseData {
|
struct FixedFramePoseData {
|
||||||
common::Time time;
|
common::Time time;
|
||||||
transform::Rigid3d pose;
|
common::optional<transform::Rigid3d> pose;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Converts 'pose_data' to a proto::FixedFramePoseData.
|
// Converts 'pose_data' to a proto::FixedFramePoseData.
|
||||||
|
|
Loading…
Reference in New Issue