[GenericPoseGraph] Add parameterization. (#1385)

master
Alexander Belyaev 2018-08-14 14:37:27 +02:00 committed by Wally B. Feed
parent 05f2c6caed
commit 5261c90c34
15 changed files with 212 additions and 38 deletions

View File

@ -14,6 +14,9 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_COMMON_UTILS_H_
#define CARTOGRAPHER_COMMON_UTILS_H_
namespace cartographer { namespace cartographer {
namespace common { namespace common {
@ -27,3 +30,5 @@ ValueType* FindOrNull(MapType& map, const KeyType& key) {
} // namespace common } // namespace common
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_COMMON_UTILS_H_

View File

@ -1,3 +1,19 @@
/*
* Copyright 2018 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/pose_graph/constraint/constraint_utils.h" #include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer { namespace cartographer {
@ -14,20 +30,26 @@ void AddPose2D(Pose2D* pose, ceres::Problem* problem) {
void AddPose3D(Pose3D* pose, ceres::Problem* problem) { void AddPose3D(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation(); auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation(); auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size()); problem->AddParameterBlock(transation->data(), transation->size(),
pose->translation_parameterization());
problem->AddParameterBlock(rotation->data(), rotation->size(),
pose->rotation_parameterization());
if (pose->constant()) { if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data()); problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data()); problem->SetParameterBlockConstant(rotation->data());
} }
} }
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) { void AddImuCalibration(ImuCalibration* imu_calibration,
auto gravity = pose->mutable_gravity_constant(); ceres::Problem* problem) {
auto orientation = pose->mutable_orientation(); auto gravity = imu_calibration->mutable_gravity_constant();
auto orientation = imu_calibration->mutable_orientation();
problem->AddParameterBlock(gravity, 1); problem->AddParameterBlock(gravity, 1);
problem->AddParameterBlock(orientation->data(), orientation->size()); problem->AddParameterBlock(orientation->data(), orientation->size(),
if (pose->constant()) { imu_calibration->orientation_parameterization());
if (imu_calibration->constant()) {
problem->SetParameterBlockConstant(gravity); problem->SetParameterBlockConstant(gravity);
problem->SetParameterBlockConstant(orientation->data()); problem->SetParameterBlockConstant(orientation->data());
} }

View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ #ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ #define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/node/imu_calibration.h" #include "cartographer/pose_graph/node/imu_calibration.h"
#include "cartographer/pose_graph/node/pose_2d.h" #include "cartographer/pose_graph/node/pose_2d.h"
#include "cartographer/pose_graph/node/pose_3d.h" #include "cartographer/pose_graph/node/pose_3d.h"
@ -31,8 +32,16 @@ void AddPose3D(Pose3D* pose, ceres::Problem* problem);
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem); void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
template <typename MapType,
typename UniquePtrType = typename MapType::mapped_type,
typename ValueTyper = typename UniquePtrType::element_type>
ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) {
auto node = common::FindOrNull(map, node_id);
return node == nullptr ? nullptr : node->get();
}
#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \ #define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \
auto node_name = common::FindOrNull(map, node_id); \ auto node_name = FindNodeOrNull(map, node_id); \
if (node_name == nullptr) { \ if (node_name == nullptr) { \
LOG(INFO) << log_message; \ LOG(INFO) << log_message; \
return; \ return; \

View File

@ -20,13 +20,12 @@ namespace cartographer {
namespace pose_graph { namespace pose_graph {
ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant, ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
const proto::ImuCalibration& imu_calibration) const proto::ImuCalibration& imu)
: Node(node_id, constant), : Node(node_id, constant),
gravity_constant_(imu_calibration.gravity_constant()), gravity_constant_(imu.gravity_constant()),
orientation_{{imu_calibration.orientation().x(), orientation_{{imu.orientation().x(), imu.orientation().y(),
imu_calibration.orientation().y(), imu.orientation().z(), imu.orientation().w()}},
imu_calibration.orientation().z(), orientation_parameterization_(imu.orientation_parameterization()) {}
imu_calibration.orientation().w()}} {}
proto::Parameters ImuCalibration::ToParametersProto() const { proto::Parameters ImuCalibration::ToParametersProto() const {
proto::Parameters parameters; proto::Parameters parameters;
@ -41,6 +40,8 @@ proto::Parameters ImuCalibration::ToParametersProto() const {
orientation->set_y(orientation_[1]); orientation->set_y(orientation_[1]);
orientation->set_z(orientation_[2]); orientation->set_z(orientation_[2]);
orientation->set_w(orientation_[3]); orientation->set_w(orientation_[3]);
imu_calibration->set_orientation_parameterization(
orientation_parameterization_.ToProto());
return parameters; return parameters;
} }

View File

@ -21,6 +21,7 @@
#include <array> #include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
@ -37,12 +38,17 @@ class ImuCalibration : public Node {
std::array<double, 4>* mutable_orientation() { return &orientation_; } std::array<double, 4>* mutable_orientation() { return &orientation_; }
const std::array<double, 4>& orientation() const { return orientation_; } const std::array<double, 4>& orientation() const { return orientation_; }
ceres::LocalParameterization* orientation_parameterization() const {
return orientation_parameterization_.ceres_parameterization();
}
protected: protected:
proto::Parameters ToParametersProto() const final; proto::Parameters ToParametersProto() const final;
private: private:
double gravity_constant_; double gravity_constant_;
std::array<double, 4> orientation_; std::array<double, 4> orientation_;
Parameterization orientation_parameterization_;
}; };
} // namespace pose_graph } // namespace pose_graph

View File

@ -31,6 +31,7 @@ constexpr char kExpectedNode[] = R"PROTO(
imu_calibration { imu_calibration {
gravity_constant: 10 gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 } orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
} }
} }
)PROTO"; )PROTO";
@ -40,6 +41,7 @@ TEST(Pose3DTest, ToProto) {
true, ParseProto<proto::ImuCalibration>(R"( true, ParseProto<proto::ImuCalibration>(R"(
gravity_constant: 10 gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 } orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
)")); )"));
EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode)); EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
} }

View File

@ -27,10 +27,10 @@ namespace cartographer {
namespace pose_graph { namespace pose_graph {
struct Nodes { struct Nodes {
// TODO(pifon): Should it really be an std::map or smth else? // TODO(pifon): Migrate to Swiss Tables when they are out.
std::map<NodeId, Pose2D> pose_2d_nodes; std::map<NodeId, std::unique_ptr<Pose2D>> pose_2d_nodes;
std::map<NodeId, Pose3D> pose_3d_nodes; std::map<NodeId, std::unique_ptr<Pose3D>> pose_3d_nodes;
std::map<NodeId, ImuCalibration> imu_calibration_nodes; std::map<NodeId, std::unique_ptr<ImuCalibration>> imu_calibration_nodes;
}; };
} // namespace pose_graph } // namespace pose_graph

View File

@ -0,0 +1,61 @@
/*
* Copyright 2018 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/pose_graph/node/parameterization/parameterization.h"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
#include "ceres/autodiff_local_parameterization.h"
#include "ceres/rotation.h"
namespace cartographer {
namespace pose_graph {
namespace {
using absl::make_unique;
using ceres::AutoDiffLocalParameterization;
using ceres::LocalParameterization;
// TODO(pifon): Check if the functors are compatible with our quaternions. Test!
std::unique_ptr<LocalParameterization> CeresParameterizationFromProto(
const proto::Parameterization& parameterization) {
switch (parameterization) {
case (proto::Parameterization::NONE):
return nullptr;
case (proto::Parameterization::YAW_ONLY):
return make_unique<AutoDiffLocalParameterization<
mapping::YawOnlyQuaternionPlus, 4, 1>>();
case (proto::Parameterization::YAW_CONSTANT):
return make_unique<AutoDiffLocalParameterization<
mapping::ConstantYawQuaternionPlus, 4, 1>>();
case (proto::Parameterization::FIX_Z):
return make_unique<ceres::SubsetParameterization>(
3, /* constant parameters */ std::vector<int>{2});
default:
LOG(FATAL) << "Parameterization is not known.";
}
return nullptr;
}
} // namespace
Parameterization::Parameterization(const proto::Parameterization& proto)
: proto_(proto),
ceres_parameterization_(CeresParameterizationFromProto(proto_)) {}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,44 @@
/*
* Copyright 2018 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_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_
#include "cartographer/pose_graph/proto/node.pb.h"
#include "ceres/local_parameterization.h"
namespace cartographer {
namespace pose_graph {
class Parameterization {
public:
explicit Parameterization(const proto::Parameterization& proto);
const proto::Parameterization& ToProto() const { return proto_; }
ceres::LocalParameterization* ceres_parameterization() const {
return ceres_parameterization_.get();
}
private:
const proto::Parameterization proto_;
const std::unique_ptr<ceres::LocalParameterization> ceres_parameterization_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_

View File

@ -24,8 +24,10 @@ Pose3D::Pose3D(const NodeId& node_id, bool constant,
: Node(node_id, constant), : Node(node_id, constant),
translation_{{pose_3d.translation().x(), pose_3d.translation().y(), translation_{{pose_3d.translation().x(), pose_3d.translation().y(),
pose_3d.translation().z()}}, pose_3d.translation().z()}},
translation_parameterization_(pose_3d.translation_parameterization()),
rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(), rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(),
pose_3d.rotation().z(), pose_3d.rotation().w()}} {} pose_3d.rotation().z(), pose_3d.rotation().w()}},
rotation_parameterization_(pose_3d.rotation_parameterization()) {}
proto::Parameters Pose3D::ToParametersProto() const { proto::Parameters Pose3D::ToParametersProto() const {
proto::Parameters parameters; proto::Parameters parameters;
@ -35,12 +37,15 @@ proto::Parameters Pose3D::ToParametersProto() const {
translation->set_x(translation_[0]); translation->set_x(translation_[0]);
translation->set_y(translation_[1]); translation->set_y(translation_[1]);
translation->set_z(translation_[2]); translation->set_z(translation_[2]);
pose_3d->set_translation_parameterization(
translation_parameterization_.ToProto());
auto* rotation = pose_3d->mutable_rotation(); auto* rotation = pose_3d->mutable_rotation();
rotation->set_x(rotation_[0]); rotation->set_x(rotation_[0]);
rotation->set_y(rotation_[1]); rotation->set_y(rotation_[1]);
rotation->set_z(rotation_[2]); rotation->set_z(rotation_[2]);
rotation->set_w(rotation_[3]); rotation->set_w(rotation_[3]);
pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto());
return parameters; return parameters;
} }

View File

@ -21,6 +21,7 @@
#include <array> #include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
@ -32,16 +33,25 @@ class Pose3D : public Node {
std::array<double, 3>* mutable_translation() { return &translation_; } std::array<double, 3>* mutable_translation() { return &translation_; }
const std::array<double, 3>& translation() const { return translation_; } const std::array<double, 3>& translation() const { return translation_; }
ceres::LocalParameterization* translation_parameterization() const {
return translation_parameterization_.ceres_parameterization();
}
std::array<double, 4>* mutable_rotation() { return &rotation_; } std::array<double, 4>* mutable_rotation() { return &rotation_; }
const std::array<double, 4>& rotation() const { return rotation_; } const std::array<double, 4>& rotation() const { return rotation_; }
ceres::LocalParameterization* rotation_parameterization() const {
return rotation_parameterization_.ceres_parameterization();
}
protected: protected:
proto::Parameters ToParametersProto() const final; proto::Parameters ToParametersProto() const final;
private: private:
std::array<double, 3> translation_; std::array<double, 3> translation_;
Parameterization translation_parameterization_;
std::array<double, 4> rotation_; std::array<double, 4> rotation_;
Parameterization rotation_parameterization_;
}; };
} // namespace pose_graph } // namespace pose_graph

View File

@ -30,6 +30,7 @@ constexpr char kExpectedNode[] = R"PROTO(
parameters { parameters {
pose_3d { pose_3d {
translation { x: 1 y: 2 z: 3 } translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 } rotation: { w: 0 x: 1 y: 2 z: 3 }
} }
} }
@ -39,7 +40,9 @@ TEST(Pose3DTest, ToProto) {
Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true, Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
ParseProto<proto::Pose3D>(R"( ParseProto<proto::Pose3D>(R"(
translation { x: 1 y: 2 z: 3 } translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 } rotation: { w: 0 x: 1 y: 2 z: 3 }
rotation_parameterization: NONE
)")); )"));
EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode)); EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
} }

View File

@ -26,6 +26,8 @@ ceres::Problem::Options CreateCeresProblemOptions() {
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership = problem_options.loss_function_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
return problem_options; return problem_options;
} }

View File

@ -28,6 +28,8 @@ namespace cartographer {
namespace pose_graph { namespace pose_graph {
namespace { namespace {
using absl::make_unique;
std::unique_ptr<Constraint> CreateConstraint( std::unique_ptr<Constraint> CreateConstraint(
const proto::Constraint& constraint) { const proto::Constraint& constraint) {
const auto& id = constraint.id(); const auto& id = constraint.id();
@ -35,22 +37,21 @@ std::unique_ptr<Constraint> CreateConstraint(
const auto& cost = constraint.cost_function(); const auto& cost = constraint.cost_function();
switch (cost.type_case()) { switch (cost.type_case()) {
case (proto::CostFunction::kRelativePose2D): case (proto::CostFunction::kRelativePose2D):
return absl::make_unique<RelativePoseConstraint2D>( return make_unique<RelativePoseConstraint2D>(id, loss,
id, loss, cost.relative_pose_2d()); cost.relative_pose_2d());
case (proto::CostFunction::kRelativePose3D): case (proto::CostFunction::kRelativePose3D):
return absl::make_unique<RelativePoseConstraint3D>( return make_unique<RelativePoseConstraint3D>(id, loss,
id, loss, cost.relative_pose_3d()); cost.relative_pose_3d());
case (proto::CostFunction::kAcceleration3D): case (proto::CostFunction::kAcceleration3D):
return absl::make_unique<AccelerationConstraint3D>( return make_unique<AccelerationConstraint3D>(id, loss,
id, loss, cost.acceleration_3d()); cost.acceleration_3d());
case (proto::CostFunction::kRotation3D): case (proto::CostFunction::kRotation3D):
return absl::make_unique<RotationContraint3D>(id, loss, return make_unique<RotationContraint3D>(id, loss, cost.rotation_3d());
cost.rotation_3d());
case (proto::CostFunction::kInterpolatedRelativePose2D): case (proto::CostFunction::kInterpolatedRelativePose2D):
return absl::make_unique<InterpolatedRelativePoseConstraint2D>( return make_unique<InterpolatedRelativePoseConstraint2D>(
id, loss, cost.interpolated_relative_pose_2d()); id, loss, cost.interpolated_relative_pose_2d());
case (proto::CostFunction::kInterpolatedRelativePose3D): case (proto::CostFunction::kInterpolatedRelativePose3D):
return absl::make_unique<InterpolatedRelativePoseConstraint3D>( return make_unique<InterpolatedRelativePoseConstraint3D>(
id, loss, cost.interpolated_relative_pose_3d()); id, loss, cost.interpolated_relative_pose_3d());
case (proto::CostFunction::TYPE_NOT_SET): case (proto::CostFunction::TYPE_NOT_SET):
LOG(FATAL) << "Constraint cost function type is not set."; LOG(FATAL) << "Constraint cost function type is not set.";
@ -65,19 +66,21 @@ void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) {
switch (node.parameters().type_case()) { switch (node.parameters().type_case()) {
case (proto::Parameters::kPose2D): { case (proto::Parameters::kPose2D): {
data->nodes.pose_2d_nodes.emplace( data->nodes.pose_2d_nodes.emplace(
node_id, node_id, make_unique<Pose2D>(node_id, node.constant(),
Pose2D(node_id, node.constant(), node.parameters().pose_2d())); node.parameters().pose_2d()));
return; return;
} }
case (proto::Parameters::kPose3D): { case (proto::Parameters::kPose3D): {
data->nodes.pose_3d_nodes.emplace( data->nodes.pose_3d_nodes.emplace(
node_id, node_id, make_unique<Pose3D>(node_id, node.constant(),
Pose3D(node_id, node.constant(), node.parameters().pose_3d())); node.parameters().pose_3d()));
return; return;
} }
case (proto::Parameters::kImuCalibration): { case (proto::Parameters::kImuCalibration): {
data->nodes.imu_calibration_nodes.emplace( data->nodes.imu_calibration_nodes.emplace(
node_id, ImuCalibration(node_id, node.constant(), node_id,
make_unique<ImuCalibration>(node_id, node.constant(),
node.parameters().imu_calibration())); node.parameters().imu_calibration()));
return; return;
} }

View File

@ -20,8 +20,9 @@ import "cartographer/transform/proto/transform.proto";
enum Parameterization { enum Parameterization {
NONE = 0; NONE = 0;
YAW_ONLY = 1; FIX_Z = 1;
YAW_CONSTANT = 2; YAW_ONLY = 2;
YAW_CONSTANT = 3;
} }
message Pose2D { message Pose2D {
@ -39,7 +40,7 @@ message Pose3D {
message ImuCalibration { message ImuCalibration {
double gravity_constant = 1; double gravity_constant = 1;
transform.proto.Quaterniond orientation = 2; transform.proto.Quaterniond orientation = 2;
Parameterization rotation_parameterization = 3; Parameterization orientation_parameterization = 3;
} }
message NodeId { message NodeId {