[GenericPoseGraph] Add interpolated constraint/cost 3d. (#1325)
* [GenericPoseGraph] Add interpolated rotation constraint/cost 3d. * Rename tests * Update include paths.master
Martin Bokeloh
GitHub
parent
d4193d4a2a
commit
f7b3d34ae5
|
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
* 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_3d.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
InterpolatedRelativePoseCost3D::InterpolatedRelativePoseCost3D(
|
||||
const proto::InterpolatedRelativePose3D::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())) {}
|
||||
|
||||
proto::InterpolatedRelativePose3D::Parameters
|
||||
InterpolatedRelativePoseCost3D::ToProto() const {
|
||||
proto::InterpolatedRelativePose3D::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_);
|
||||
return parameters;
|
||||
}
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
* 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_3D_H_
|
||||
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_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 interpolated relative pose in 3d and
|
||||
// de/serialization methods; only the first pose is linearly interpolated
|
||||
// between two nodes.
|
||||
class InterpolatedRelativePoseCost3D {
|
||||
public:
|
||||
InterpolatedRelativePoseCost3D(
|
||||
const proto::InterpolatedRelativePose3D::Parameters& parameters);
|
||||
|
||||
proto::InterpolatedRelativePose3D::Parameters ToProto() const;
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const first_start_translation,
|
||||
const T* const first_start_rotation,
|
||||
const T* const first_end_translation,
|
||||
const T* const first_end_rotation,
|
||||
const T* const second_translation,
|
||||
const T* const second_rotation, T* const error_out) const {
|
||||
const std::tuple<std::array<T, 4>, std::array<T, 3>>
|
||||
interpolated_rotation_and_translation =
|
||||
mapping::optimization::InterpolateNodes3D(
|
||||
first_start_rotation, first_start_translation,
|
||||
first_end_rotation, first_end_translation,
|
||||
interpolation_factor_);
|
||||
const std::array<T, 6> error = mapping::optimization::ScaleError(
|
||||
mapping::optimization::ComputeUnscaledError(
|
||||
first_T_second_,
|
||||
std::get<0>(interpolated_rotation_and_translation).data(),
|
||||
std::get<1>(interpolated_rotation_and_translation).data(),
|
||||
second_rotation, second_translation),
|
||||
translation_weight_, rotation_weight_);
|
||||
std::copy(std::begin(error), std::end(error), error_out);
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
const double translation_weight_;
|
||||
const double rotation_weight_;
|
||||
const double interpolation_factor_;
|
||||
const transform::Rigid3d first_T_second_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_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_3d.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 PositionType = 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: 10
|
||||
interpolation_factor: 0.3
|
||||
)PROTO";
|
||||
|
||||
TEST(InterpolatedRelativePoseCost3DTest, SerializesCorrectly) {
|
||||
const auto parameters =
|
||||
ParseProto<proto::InterpolatedRelativePose3D::Parameters>(kParameters);
|
||||
InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters);
|
||||
const auto actual_proto = interpolated_relative_pose_cost_3d.ToProto();
|
||||
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
|
||||
}
|
||||
|
||||
TEST(InterpolatedRelativePoseCost3DTest, EvaluatesCorrectly) {
|
||||
const auto parameters =
|
||||
ParseProto<proto::InterpolatedRelativePose3D::Parameters>(kParameters);
|
||||
InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters);
|
||||
|
||||
const PositionType kFirstStartPosition{{1., 1., 1.}};
|
||||
const RotationType kFirstStartRotation{{1., 1., 1., 1.}};
|
||||
const PositionType kFirstEndPosition{{2., 3., 4.}};
|
||||
const RotationType kFirstEndRotation{{1.1, 1.2, 1.3, 1.4}};
|
||||
const PositionType kSecondPosition{{0., -1., -2.}};
|
||||
const RotationType kSecondRotation{{.1, .2, .3, .4}};
|
||||
|
||||
ResidualType residuals;
|
||||
EXPECT_TRUE(interpolated_relative_pose_cost_3d(
|
||||
kFirstStartPosition.data(), kFirstStartRotation.data(),
|
||||
kFirstEndPosition.data(), kFirstEndRotation.data(),
|
||||
kSecondPosition.data(), kSecondRotation.data(), residuals.data()));
|
||||
EXPECT_THAT(residuals,
|
||||
ElementsAre(Near(8.4594), Near(10.27735), Near(-4.45472),
|
||||
Near(0.968852), Near(11.96531), Near(3.34254)));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
/*
|
||||
* 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_3d.h"
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/utils.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
void AddPoseParameters(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());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
|
||||
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
|
||||
const proto::InterpolatedRelativePose3D& proto)
|
||||
: Constraint(id, loss_function_proto),
|
||||
first_start_(proto.first_start()),
|
||||
first_end_(proto.first_end()),
|
||||
second_(proto.second()),
|
||||
cost_(new InterpolatedRelativePoseCost3D(proto.parameters())),
|
||||
ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
|
||||
|
||||
void InterpolatedRelativePoseConstraint3D::AddToOptimizer(
|
||||
Nodes* nodes, ceres::Problem* problem) const {
|
||||
auto first_node_start =
|
||||
common::FindOrNull(nodes->pose_3d_nodes, first_start_);
|
||||
if (first_node_start == nullptr) {
|
||||
LOG(INFO) << "First node (start) was not found in pose_3d_nodes.";
|
||||
return;
|
||||
}
|
||||
|
||||
auto first_node_end = common::FindOrNull(nodes->pose_3d_nodes, first_end_);
|
||||
if (first_node_end == nullptr) {
|
||||
LOG(INFO) << "First node (end) was not found in pose_3d_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;
|
||||
}
|
||||
|
||||
AddPoseParameters(first_node_start, problem);
|
||||
AddPoseParameters(first_node_end, problem);
|
||||
AddPoseParameters(second_node, problem);
|
||||
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
|
||||
first_node_start->mutable_translation()->data(),
|
||||
first_node_start->mutable_rotation()->data(),
|
||||
first_node_end->mutable_translation()->data(),
|
||||
first_node_end->mutable_rotation()->data(),
|
||||
second_node->mutable_translation()->data(),
|
||||
second_node->mutable_rotation()->data());
|
||||
}
|
||||
|
||||
proto::CostFunction InterpolatedRelativePoseConstraint3D::ToCostFunctionProto()
|
||||
const {
|
||||
proto::CostFunction cost_function;
|
||||
auto* interpolated_relative_pose_3d =
|
||||
cost_function.mutable_interpolated_relative_pose_3d();
|
||||
*interpolated_relative_pose_3d->mutable_first_start() =
|
||||
first_start_.ToProto();
|
||||
*interpolated_relative_pose_3d->mutable_first_end() = first_end_.ToProto();
|
||||
*interpolated_relative_pose_3d->mutable_second() = second_.ToProto();
|
||||
*interpolated_relative_pose_3d->mutable_parameters() = cost_->ToProto();
|
||||
return cost_function;
|
||||
}
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -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.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_
|
||||
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_
|
||||
|
||||
#include "cartographer/pose_graph/constraint/constraint.h"
|
||||
#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
class InterpolatedRelativePoseConstraint3D : public Constraint {
|
||||
public:
|
||||
InterpolatedRelativePoseConstraint3D(
|
||||
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
|
||||
const proto::InterpolatedRelativePose3D& proto);
|
||||
|
||||
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
|
||||
|
||||
protected:
|
||||
proto::CostFunction ToCostFunctionProto() const final;
|
||||
|
||||
private:
|
||||
// clang-format off
|
||||
using AutoDiffFunction = ceres::AutoDiffCostFunction<
|
||||
InterpolatedRelativePoseCost3D,
|
||||
6 /* number of residuals */,
|
||||
3 /* translation of first start pose */,
|
||||
4 /* rotation of first start pose */,
|
||||
3 /* translation of first end pose */,
|
||||
4 /* rotation 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.
|
||||
InterpolatedRelativePoseCost3D* const cost_;
|
||||
std::unique_ptr<AutoDiffFunction> ceres_cost_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
/*
|
||||
* 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_3d.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 PositionType = std::array<double, 3>;
|
||||
using RotationType = std::array<double, 4>;
|
||||
using ResidualType = std::array<double, 3>;
|
||||
|
||||
constexpr char kConstraint[] = R"PROTO(
|
||||
id: "narf"
|
||||
cost_function {
|
||||
interpolated_relative_pose_3d {
|
||||
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
|
||||
interpolation_factor: 0.3
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
loss_function { quadratic_loss {} }
|
||||
)PROTO";
|
||||
|
||||
TEST(InterpolatedRelativePostConstraint3DTest, SerializesCorrectly) {
|
||||
const auto proto = ParseProto<proto::Constraint>(kConstraint);
|
||||
InterpolatedRelativePoseConstraint3D constraint(
|
||||
proto.id(), proto.loss_function(),
|
||||
proto.cost_function().interpolated_relative_pose_3d());
|
||||
const auto actual_proto = constraint.ToProto();
|
||||
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -88,19 +88,19 @@ message RelativePose2DInterpolated {
|
|||
Parameters parameters = 3;
|
||||
}
|
||||
|
||||
message RelativePose3DInterpolated {
|
||||
InterpolatedNode first = 1;
|
||||
NodeId second = 2;
|
||||
message InterpolatedRelativePose3D {
|
||||
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_start = 4;
|
||||
transform.proto.Quaterniond gravity_alignment_end = 5;
|
||||
// Interpolates between first_start and first_end.
|
||||
double interpolation_factor = 4;
|
||||
}
|
||||
Parameters parameters = 3;
|
||||
Parameters parameters = 4;
|
||||
}
|
||||
|
||||
message CostFunction {
|
||||
|
|
@ -110,6 +110,6 @@ message CostFunction {
|
|||
Acceleration3D acceleration_3d = 3;
|
||||
Rotation3D rotation_3d = 4;
|
||||
RelativePose2DInterpolated relative_pose_2d_interpolated = 5;
|
||||
RelativePose3DInterpolated relative_pose_3d_interpolated = 6;
|
||||
InterpolatedRelativePose3D interpolated_relative_pose_3d = 6;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue