[GenericPoseGraph] Add InterpolatedRelativePose2D constraint. (#1337)
Alexander Belyaev
Wally B. Feed
parent
e87100a3ad
commit
c2c341397f
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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_
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue