[GenericPoseGraph] Add acceleration constraint. (#1319)
Alexander Belyaev
GitHub
parent
8250264441
commit
d4193d4a2a
|
|
@ -0,0 +1,118 @@
|
|||
/*
|
||||
* 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/acceleration_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());
|
||||
}
|
||||
}
|
||||
|
||||
void AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) {
|
||||
auto gravity = pose->mutable_gravity_constant();
|
||||
auto orientation = pose->mutable_orientation();
|
||||
problem->AddParameterBlock(gravity, 1);
|
||||
problem->AddParameterBlock(orientation->data(), orientation->size());
|
||||
if (pose->constant()) {
|
||||
problem->SetParameterBlockConstant(gravity);
|
||||
problem->SetParameterBlockConstant(orientation->data());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
AccelerationConstraint3D::AccelerationConstraint3D(
|
||||
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
|
||||
const proto::Acceleration3D& proto)
|
||||
: Constraint(id, loss_function_proto),
|
||||
first_(proto.first()),
|
||||
second_(proto.second()),
|
||||
third_(proto.third()),
|
||||
imu_(proto.imu_calibration()),
|
||||
cost_(new AccelerationCost3D(proto.parameters())),
|
||||
ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
|
||||
|
||||
void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
|
||||
ceres::Problem* problem) const {
|
||||
auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
|
||||
if (first_node == nullptr) {
|
||||
LOG(INFO) << "First node 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;
|
||||
}
|
||||
|
||||
auto third_node = common::FindOrNull(nodes->pose_3d_nodes, third_);
|
||||
if (third_node == nullptr) {
|
||||
LOG(INFO) << "Third node was not found in pose_3d_nodes.";
|
||||
return;
|
||||
}
|
||||
|
||||
auto imu_node = common::FindOrNull(nodes->imu_calibration_nodes, imu_);
|
||||
if (imu_node == nullptr) {
|
||||
LOG(INFO) << "IMU calibration node was not found in imu_calibration_nodes.";
|
||||
return;
|
||||
}
|
||||
|
||||
if (first_node->constant() && second_node->constant() &&
|
||||
third_node->constant() && imu_node->constant()) {
|
||||
LOG(INFO) << "All nodes are constant, skipping the constraint.";
|
||||
return;
|
||||
}
|
||||
|
||||
AddPoseParameters(first_node, problem);
|
||||
AddPoseParameters(second_node, problem);
|
||||
AddPoseParameters(third_node, problem);
|
||||
AddImuParameters(imu_node, problem);
|
||||
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
|
||||
second_node->mutable_rotation()->data(),
|
||||
second_node->mutable_translation()->data(),
|
||||
first_node->mutable_translation()->data(),
|
||||
third_node->mutable_translation()->data(),
|
||||
imu_node->mutable_gravity_constant(),
|
||||
imu_node->mutable_orientation()->data());
|
||||
}
|
||||
|
||||
proto::CostFunction AccelerationConstraint3D::ToCostFunctionProto() const {
|
||||
proto::CostFunction cost_function;
|
||||
auto* acceleration_3d = cost_function.mutable_acceleration_3d();
|
||||
*acceleration_3d->mutable_first() = first_.ToProto();
|
||||
*acceleration_3d->mutable_second() = second_.ToProto();
|
||||
*acceleration_3d->mutable_third() = third_.ToProto();
|
||||
*acceleration_3d->mutable_imu_calibration() = imu_.ToProto();
|
||||
*acceleration_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_ACCELERATION_CONSTRAINT_3D_H_
|
||||
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_
|
||||
|
||||
#include "cartographer/pose_graph/constraint/constraint.h"
|
||||
#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
class AccelerationConstraint3D : public Constraint {
|
||||
public:
|
||||
AccelerationConstraint3D(const ConstraintId& id,
|
||||
const proto::LossFunction& loss_function_proto,
|
||||
const proto::Acceleration3D& proto);
|
||||
|
||||
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
|
||||
|
||||
protected:
|
||||
proto::CostFunction ToCostFunctionProto() const final;
|
||||
|
||||
private:
|
||||
// clang-format off
|
||||
using AutoDiffFunction =
|
||||
ceres::AutoDiffCostFunction<AccelerationCost3D,
|
||||
3 /* residuals */,
|
||||
4 /* rotation variables second pose*/,
|
||||
3 /* position variables second pose*/,
|
||||
3 /* position variables first pose */,
|
||||
3 /* position variables second pose */,
|
||||
1 /* gravity variable */,
|
||||
4 /* imu orientation */>;
|
||||
// clang-format on
|
||||
NodeId first_;
|
||||
NodeId second_;
|
||||
NodeId third_;
|
||||
NodeId imu_;
|
||||
// The cost function is owned by the ceres cost function.
|
||||
AccelerationCost3D* const cost_;
|
||||
std::unique_ptr<AutoDiffFunction> ceres_cost_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
/*
|
||||
* 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/acceleration_constraint_3d.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: "hal_acceleration"
|
||||
cost_function {
|
||||
acceleration_3d {
|
||||
first { object_id: "hal9000" timestamp: 100 }
|
||||
second { object_id: "hal9000" timestamp: 200 }
|
||||
third { object_id: "hal9000" timestamp: 300 }
|
||||
imu_calibration { object_id: "hal_imu" }
|
||||
parameters {
|
||||
delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
|
||||
first_to_second_delta_time_seconds: 10.0
|
||||
second_to_third_delta_time_seconds: 20.0
|
||||
scaling_factor: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
loss_function { quadratic_loss {} }
|
||||
)PROTO";
|
||||
|
||||
TEST(AccelerationConstraint3DTest, SerializesCorrectly) {
|
||||
const auto proto = ParseProto<proto::Constraint>(kConstraint);
|
||||
AccelerationConstraint3D constraint(proto.id(), proto.loss_function(),
|
||||
proto.cost_function().acceleration_3d());
|
||||
const auto actual_proto = constraint.ToProto();
|
||||
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
* 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/acceleration_cost_3d.h"
|
||||
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
AccelerationCost3D::AccelerationCost3D(
|
||||
const proto::Acceleration3D::Parameters& parameters)
|
||||
: delta_velocity_imu_frame_(
|
||||
transform::ToEigen(parameters.delta_velocity_imu_frame())),
|
||||
first_to_second_delta_time_seconds_(
|
||||
parameters.first_to_second_delta_time_seconds()),
|
||||
second_to_third_delta_time_seconds_(
|
||||
parameters.second_to_third_delta_time_seconds()),
|
||||
scaling_factor_(parameters.scaling_factor()) {}
|
||||
|
||||
proto::Acceleration3D::Parameters AccelerationCost3D::ToProto() const {
|
||||
proto::Acceleration3D::Parameters parameters;
|
||||
*parameters.mutable_delta_velocity_imu_frame() =
|
||||
transform::ToProto(delta_velocity_imu_frame_);
|
||||
parameters.set_first_to_second_delta_time_seconds(
|
||||
first_to_second_delta_time_seconds_);
|
||||
parameters.set_second_to_third_delta_time_seconds(
|
||||
second_to_third_delta_time_seconds_);
|
||||
parameters.set_scaling_factor(scaling_factor_);
|
||||
return parameters;
|
||||
}
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
/*
|
||||
* 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_ACCELERATION_COST_3D_H_
|
||||
#define CARTOGRAPHER_ACCELERATION_COST_3D_H_
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/pose_graph/proto/cost_function.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
// Penalizes differences between IMU data and optimized accelerations.
|
||||
class AccelerationCost3D {
|
||||
public:
|
||||
explicit AccelerationCost3D(
|
||||
const proto::Acceleration3D::Parameters& parameters);
|
||||
|
||||
proto::Acceleration3D::Parameters ToProto() const;
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const middle_rotation, const T* const start_position,
|
||||
const T* const middle_position, const T* const end_position,
|
||||
const T* const gravity_constant,
|
||||
const T* const imu_calibration, T* residual) const {
|
||||
using Vector3T = Eigen::Matrix<T, 3, 1>;
|
||||
using TranslationMap = Eigen::Map<const Vector3T>;
|
||||
using RotationMap = Eigen::Map<const Eigen::Quaternion<T>>;
|
||||
|
||||
const Vector3T imu_delta_velocity =
|
||||
RotationMap(middle_rotation) * RotationMap(imu_calibration) *
|
||||
delta_velocity_imu_frame_.cast<T>() -
|
||||
(*gravity_constant * 0.5 *
|
||||
(first_to_second_delta_time_seconds_ +
|
||||
second_to_third_delta_time_seconds_)) *
|
||||
Eigen::Vector3d::UnitZ().cast<T>();
|
||||
|
||||
const Vector3T start_velocity =
|
||||
(TranslationMap(middle_position) - TranslationMap(start_position)) /
|
||||
T(first_to_second_delta_time_seconds_);
|
||||
const Vector3T end_velocity =
|
||||
(TranslationMap(end_position) - TranslationMap(middle_position)) /
|
||||
T(second_to_third_delta_time_seconds_);
|
||||
const Vector3T delta_velocity = end_velocity - start_velocity;
|
||||
|
||||
(Eigen::Map<Vector3T>(residual) =
|
||||
T(scaling_factor_) * (imu_delta_velocity - delta_velocity));
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
const Eigen::Vector3d delta_velocity_imu_frame_;
|
||||
const double first_to_second_delta_time_seconds_;
|
||||
const double second_to_third_delta_time_seconds_;
|
||||
const double scaling_factor_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_ACCELERATION_COST_3D_H_
|
||||
|
|
@ -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.
|
||||
*/
|
||||
|
||||
#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
|
||||
|
||||
#include "cartographer/testing/test_helpers.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
using ::google::protobuf::TextFormat;
|
||||
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 kParameters[] = R"PROTO(
|
||||
delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
|
||||
first_to_second_delta_time_seconds: 10.0
|
||||
second_to_third_delta_time_seconds: 20.0
|
||||
scaling_factor: 2.0
|
||||
)PROTO";
|
||||
|
||||
TEST(AccelerationCost3DTest, SerializesCorrectly) {
|
||||
AccelerationCost3D acceleration_cost_3d(
|
||||
ParseProto<proto::Acceleration3D::Parameters>(kParameters));
|
||||
EXPECT_THAT(acceleration_cost_3d.ToProto(), EqualsProto(kParameters));
|
||||
}
|
||||
|
||||
TEST(AccelerationCost3DTest, EvaluatesCorrectly) {
|
||||
AccelerationCost3D acceleration_cost_3d(
|
||||
ParseProto<proto::Acceleration3D::Parameters>(kParameters));
|
||||
const PositionType kStartPosition{{1., 1., 1.}};
|
||||
|
||||
const PositionType kMiddlePosition{{2., 2., 2.}};
|
||||
const RotationType kMiddleRotation{{0., 0., 0., 1.}};
|
||||
|
||||
const PositionType kEndPosition{{3., 3., 3.}};
|
||||
|
||||
const double kGravityConstant{10.};
|
||||
const RotationType kImuCalibration{{0., 0., 0., 1.0}};
|
||||
|
||||
ResidualType residuals;
|
||||
EXPECT_TRUE(acceleration_cost_3d(
|
||||
kMiddleRotation.data(), kStartPosition.data(), kMiddlePosition.data(),
|
||||
kEndPosition.data(), &kGravityConstant, kImuCalibration.data(),
|
||||
residuals.data()));
|
||||
EXPECT_THAT(residuals, ElementsAre(Near(2.1), Near(2.1), Near(-297.9)));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -51,8 +51,8 @@ message Acceleration3D {
|
|||
|
||||
message Parameters {
|
||||
transform.proto.Vector3d delta_velocity_imu_frame = 1;
|
||||
int64 first_to_second_time_delta = 2;
|
||||
int64 second_to_third_time_delta = 3;
|
||||
double first_to_second_delta_time_seconds = 2;
|
||||
double second_to_third_delta_time_seconds = 3;
|
||||
double scaling_factor = 4;
|
||||
}
|
||||
Parameters parameters = 5;
|
||||
|
|
|
|||
Loading…
Reference in New Issue