[GenericPoseGraph] Add InterpolatedRelativePose2D constraint. (#1337)

master
Alexander Belyaev 2018-07-26 15:00:13 +02:00 committed by Wally B. Feed
parent e87100a3ad
commit c2c341397f
7 changed files with 429 additions and 11 deletions

View File

@ -0,0 +1,48 @@
/*
* 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/cost_function/interpolated_relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
InterpolatedRelativePoseCost2D::InterpolatedRelativePoseCost2D(
const proto::InterpolatedRelativePose2D::Parameters& parameters)
: translation_weight_(parameters.translation_weight()),
rotation_weight_(parameters.rotation_weight()),
interpolation_factor_(parameters.interpolation_factor()),
first_T_second_(transform::ToRigid3(parameters.first_t_second())),
gravity_alignment_first_start_(
transform::ToEigen(parameters.gravity_alignment_first_start())),
gravity_alignment_first_end_(
transform::ToEigen(parameters.gravity_alignment_first_end())) {}
proto::InterpolatedRelativePose2D::Parameters
InterpolatedRelativePoseCost2D::ToProto() const {
proto::InterpolatedRelativePose2D::Parameters parameters;
parameters.set_translation_weight(translation_weight_);
parameters.set_rotation_weight(rotation_weight_);
parameters.set_interpolation_factor(interpolation_factor_);
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
*parameters.mutable_gravity_alignment_first_start() =
transform::ToProto(gravity_alignment_first_start_);
*parameters.mutable_gravity_alignment_first_end() =
transform::ToProto(gravity_alignment_first_end_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,66 @@
/*
* 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_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
// Provides cost function for relative pose and de/serialization methods.
class InterpolatedRelativePoseCost2D {
public:
explicit InterpolatedRelativePoseCost2D(
const proto::InterpolatedRelativePose2D::Parameters& parameters);
proto::InterpolatedRelativePose2D::Parameters ToProto() const;
template <typename T>
bool operator()(const T* const first_start_pose,
const T* const first_end_pose,
const T* const second_translation,
const T* const second_rotation, T* const e) const {
const std::tuple<std::array<T, 4>, std::array<T, 3>>
interpolated_first_pose = mapping::optimization::InterpolateNodes2D(
first_start_pose, gravity_alignment_first_start_, first_end_pose,
gravity_alignment_first_end_, interpolation_factor_);
const std::array<T, 6> error = mapping::optimization::ScaleError(
mapping::optimization::ComputeUnscaledError(
first_T_second_, std::get<0>(interpolated_first_pose).data(),
std::get<1>(interpolated_first_pose).data(), second_rotation,
second_translation),
translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), e);
return true;
}
private:
const double translation_weight_;
const double rotation_weight_;
const double interpolation_factor_;
const transform::Rigid3d first_T_second_;
const Eigen::Quaterniond gravity_alignment_first_start_;
const Eigen::Quaterniond gravity_alignment_first_end_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H

View File

@ -0,0 +1,76 @@
/*
* 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/cost_function/interpolated_relative_pose_cost_2d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near;
using testing::ParseProto;
using Position2DType = std::array<double, 3>;
using TranslationType = std::array<double, 3>;
using RotationType = std::array<double, 4>;
using ResidualType = std::array<double, 6>;
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 1
rotation_weight: 2
gravity_alignment_first_start { x: 0.4 y: 0.1 z: 0.3 w: 0.2 }
gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
interpolation_factor: 0.3
)PROTO";
TEST(InterpolatedRelativePoseCost2DTest, SerializesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
const auto actual_proto = interpolated_relative_pose_cost_2d.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
}
TEST(InterpolatedRelativePoseCost2DTest, EvaluatesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
const Position2DType kFirstStartPose{{1., 1., 1.}};
const Position2DType kFirstEndPose{{2., 3., 4.}};
const TranslationType kSecondTranslation{{0., -1., -2.}};
const RotationType kSecondRotation{{.4, .2, .3, .1}};
ResidualType residuals;
EXPECT_TRUE(interpolated_relative_pose_cost_2d(
kFirstStartPose.data(), kFirstEndPose.data(), kSecondTranslation.data(),
kSecondRotation.data(), residuals.data()));
EXPECT_THAT(residuals,
ElementsAre(Near(4.49044), Near(1.8527), Near(3.49511),
Near(-1.93746), Near(3.54854), Near(4.50243)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,108 @@
/*
* 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/interpolated_relative_pose_constraint_2d.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddPose3DParameters(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddPose2DParameters(Pose2D* pose, ceres::Problem* problem) {
auto pose_2d = pose->mutable_pose_2d();
problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(pose_2d->data());
}
}
} // namespace
InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose2D& proto)
: Constraint(id, loss_function_proto),
first_start_(proto.first_start()),
first_end_(proto.first_end()),
second_(proto.second()),
cost_(new InterpolatedRelativePoseCost2D(proto.parameters())),
ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
Nodes* nodes, ceres::Problem* problem) const {
auto first_node_start =
common::FindOrNull(nodes->pose_2d_nodes, first_start_);
if (first_node_start == nullptr) {
LOG(INFO) << "First node (start) was not found in pose_2d_nodes.";
return;
}
auto first_node_end = common::FindOrNull(nodes->pose_2d_nodes, first_end_);
if (first_node_end == nullptr) {
LOG(INFO) << "First node (end) was not found in pose_2d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
if (first_node_start->constant() && first_node_end->constant() &&
second_node->constant()) {
LOG(INFO) << "All nodes are constant, skipping the constraint.";
return;
}
AddPose2DParameters(first_node_start, problem);
AddPose2DParameters(first_node_end, problem);
AddPose3DParameters(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
first_node_start->mutable_pose_2d()->data(),
first_node_end->mutable_pose_2d()->data(),
second_node->mutable_translation()->data(),
second_node->mutable_rotation()->data());
}
proto::CostFunction InterpolatedRelativePoseConstraint2D::ToCostFunctionProto()
const {
proto::CostFunction cost_function;
auto* interpolated_relative_pose_2d =
cost_function.mutable_interpolated_relative_pose_2d();
*interpolated_relative_pose_2d->mutable_first_start() =
first_start_.ToProto();
*interpolated_relative_pose_2d->mutable_first_end() = first_end_.ToProto();
*interpolated_relative_pose_2d->mutable_second() = second_.ToProto();
*interpolated_relative_pose_2d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,59 @@
/*
* 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_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
class InterpolatedRelativePoseConstraint2D : public Constraint {
public:
InterpolatedRelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose2D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
// clang-format off
using AutoDiffFunction = ceres::AutoDiffCostFunction<
InterpolatedRelativePoseCost2D,
6 /* residuals */,
3 /* 2d pose variables of first start pose */,
3 /* 2d pose variables of first end pose */,
3 /* translation of second pose */,
4 /* rotation of second pose */>;
// clang-format on
NodeId first_start_;
NodeId first_end_;
NodeId second_;
// The cost function is owned by the ceres cost function.
InterpolatedRelativePoseCost2D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_

View File

@ -0,0 +1,62 @@
/*
* 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/interpolated_relative_pose_constraint_2d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::EqualsProto;
using testing::ParseProto;
constexpr char kConstraint[] = R"PROTO(
id: "narf"
cost_function {
interpolated_relative_pose_2d {
first_start { object_id: "node0_start" }
first_end { object_id: "node0_end" }
second { object_id: "node1" }
parameters {
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 0.5
rotation_weight: 1.0
gravity_alignment_first_start { x: 0.4 y: 0.2 z: 0.3 w: 0.2 }
gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
interpolation_factor: 0.3
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(InterpolatedRelativePostConstraint2DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
InterpolatedRelativePoseConstraint2D constraint(
proto.id(), proto.loss_function(),
proto.cost_function().interpolated_relative_pose_2d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

View File

@ -70,22 +70,21 @@ message Rotation3D {
Parameters parameters = 4;
}
message InterpolatedNode {
NodeId start = 1;
NodeId end = 2;
double factor = 3;
}
message RelativePose2DInterpolated {
InterpolatedNode first = 1;
NodeId second = 2;
message InterpolatedRelativePose2D {
NodeId first_start = 1;
NodeId first_end = 2;
NodeId second = 3;
message Parameters {
transform.proto.Rigid3d first_t_second = 1;
double translation_weight = 2;
double rotation_weight = 3;
transform.proto.Quaterniond gravity_alignment_first_start = 4;
transform.proto.Quaterniond gravity_alignment_first_end = 5;
// Interpolates between first_start and first_end.
double interpolation_factor = 6;
}
Parameters parameters = 3;
Parameters parameters = 4;
}
message InterpolatedRelativePose3D {
@ -109,7 +108,7 @@ message CostFunction {
RelativePose3D relative_pose_3d = 2;
Acceleration3D acceleration_3d = 3;
Rotation3D rotation_3d = 4;
RelativePose2DInterpolated relative_pose_2d_interpolated = 5;
InterpolatedRelativePose2D interpolated_relative_pose_2d = 5;
InterpolatedRelativePose3D interpolated_relative_pose_3d = 6;
}
}