[GenericPoseGraph] Add interpolated constraint/cost 3d. (#1325)
* [GenericPoseGraph] Add interpolated rotation constraint/cost 3d. * Rename tests * Update include paths.master
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;
|
Parameters parameters = 3;
|
||||||
}
|
}
|
||||||
|
|
||||||
message RelativePose3DInterpolated {
|
message InterpolatedRelativePose3D {
|
||||||
InterpolatedNode first = 1;
|
NodeId first_start = 1;
|
||||||
NodeId second = 2;
|
NodeId first_end = 2;
|
||||||
|
NodeId second = 3;
|
||||||
|
|
||||||
message Parameters {
|
message Parameters {
|
||||||
transform.proto.Rigid3d first_t_second = 1;
|
transform.proto.Rigid3d first_t_second = 1;
|
||||||
double translation_weight = 2;
|
double translation_weight = 2;
|
||||||
double rotation_weight = 3;
|
double rotation_weight = 3;
|
||||||
|
// Interpolates between first_start and first_end.
|
||||||
transform.proto.Quaterniond gravity_alignment_start = 4;
|
double interpolation_factor = 4;
|
||||||
transform.proto.Quaterniond gravity_alignment_end = 5;
|
|
||||||
}
|
}
|
||||||
Parameters parameters = 3;
|
Parameters parameters = 4;
|
||||||
}
|
}
|
||||||
|
|
||||||
message CostFunction {
|
message CostFunction {
|
||||||
|
@ -110,6 +110,6 @@ message CostFunction {
|
||||||
Acceleration3D acceleration_3d = 3;
|
Acceleration3D acceleration_3d = 3;
|
||||||
Rotation3D rotation_3d = 4;
|
Rotation3D rotation_3d = 4;
|
||||||
RelativePose2DInterpolated relative_pose_2d_interpolated = 5;
|
RelativePose2DInterpolated relative_pose_2d_interpolated = 5;
|
||||||
RelativePose3DInterpolated relative_pose_3d_interpolated = 6;
|
InterpolatedRelativePose3D interpolated_relative_pose_3d = 6;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue