Implement LandmarkCostFunction 3D. (#860)
Alexander Belyaev
GitHub
parent
a7ed7e224f
commit
93568641f9
|
|
@ -0,0 +1,140 @@
|
|||
/*
|
||||
* 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_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
||||
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/mapping/pose_graph_interface.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "ceres/ceres.h"
|
||||
#include "ceres/jet.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace pose_graph {
|
||||
|
||||
template <typename T>
|
||||
std::array<T, 4> SlerpQuaternions(const T* const prev_rotation,
|
||||
const T* const next_rotation, T factor) {
|
||||
// Angle 'theta' is the half-angle "between" quaternions. It can be computed
|
||||
// as the arccosine of their dot product.
|
||||
const T cos_theta = prev_rotation[0] * next_rotation[0] +
|
||||
prev_rotation[1] * next_rotation[1] +
|
||||
prev_rotation[2] * next_rotation[2] +
|
||||
prev_rotation[3] * next_rotation[3];
|
||||
// If numerical error brings 'cos_theta' outside of [-1., 1.] interval, then
|
||||
// the quaternions are likely to be collinear.
|
||||
if (cos_theta >= T(1.0) || cos_theta <= T(-1.0)) {
|
||||
return {{next_rotation[0], next_rotation[1], next_rotation[2],
|
||||
next_rotation[3]}};
|
||||
}
|
||||
const T theta = acos(abs(cos_theta));
|
||||
const T sin_theta = sin(theta);
|
||||
const T prev_scale = sin((T(1.0) - factor) * theta) / sin_theta;
|
||||
const T next_scale =
|
||||
sin(factor * theta) * (cos_theta < T(0) ? T(-1.0) : T(1.0)) / sin_theta;
|
||||
|
||||
return {{prev_scale * prev_rotation[0] + next_scale * next_rotation[0],
|
||||
prev_scale * prev_rotation[1] + next_scale * next_rotation[1],
|
||||
prev_scale * prev_rotation[2] + next_scale * next_rotation[2],
|
||||
prev_scale * prev_rotation[3] + next_scale * next_rotation[3]}};
|
||||
}
|
||||
|
||||
// Cost function measuring the weighted error between the observed pose given by
|
||||
// the landmark measurement and the linearly interpolated pose.
|
||||
class LandmarkCostFunction {
|
||||
public:
|
||||
using LandmarkObservation =
|
||||
mapping::PoseGraph::LandmarkNode::LandmarkObservation;
|
||||
|
||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||
const LandmarkObservation& observation, common::Time prev_node_time,
|
||||
common::Time next_node_time) {
|
||||
return new ceres::AutoDiffCostFunction<
|
||||
LandmarkCostFunction, 6 /* residuals */,
|
||||
4 /* previous node rotation variables */,
|
||||
3 /* previous node translation variables */,
|
||||
4 /* next node rotation variables */,
|
||||
3 /* next node translation variables */,
|
||||
4 /* landmark rotation variables */,
|
||||
3 /* landmark translation variables */>(
|
||||
new LandmarkCostFunction(observation, prev_node_time, next_node_time));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const prev_node_rotation,
|
||||
const T* const prev_node_translation,
|
||||
const T* const next_node_rotation,
|
||||
const T* const next_node_translation,
|
||||
const T* const landmark_rotation,
|
||||
const T* const landmark_translation, T* const e) const {
|
||||
const T interpolated_pose_translation[3] = {
|
||||
prev_node_translation[0] +
|
||||
interpolation_parameter_ *
|
||||
(next_node_translation[0] - prev_node_translation[0]),
|
||||
prev_node_translation[1] +
|
||||
interpolation_parameter_ *
|
||||
(next_node_translation[1] - prev_node_translation[1]),
|
||||
prev_node_translation[2] +
|
||||
interpolation_parameter_ *
|
||||
(next_node_translation[2] - prev_node_translation[2])};
|
||||
|
||||
std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
|
||||
prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
|
||||
|
||||
// TODO(pifon2a): Move functions common for all cost functions outside of
|
||||
// SpaCostFunction scope.
|
||||
const std::array<T, 6> unscaled_error =
|
||||
mapping_3d::pose_graph::SpaCostFunction::ComputeUnscaledError(
|
||||
landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
|
||||
interpolated_pose_translation, landmark_rotation,
|
||||
landmark_translation);
|
||||
|
||||
e[0] = T(translation_weight_) * unscaled_error[0];
|
||||
e[1] = T(translation_weight_) * unscaled_error[1];
|
||||
e[2] = T(translation_weight_) * unscaled_error[2];
|
||||
e[3] = T(rotation_weight_) * unscaled_error[3];
|
||||
e[4] = T(rotation_weight_) * unscaled_error[4];
|
||||
e[5] = T(rotation_weight_) * unscaled_error[5];
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
LandmarkCostFunction(const LandmarkObservation& observation,
|
||||
common::Time prev_node_time, common::Time next_node_time)
|
||||
: landmark_to_tracking_transform_(
|
||||
observation.landmark_to_tracking_transform),
|
||||
translation_weight_(observation.translation_weight),
|
||||
rotation_weight_(observation.rotation_weight),
|
||||
interpolation_parameter_(
|
||||
common::ToSeconds(observation.time - prev_node_time) /
|
||||
common::ToSeconds(next_node_time - prev_node_time)) {}
|
||||
|
||||
const transform::Rigid3d landmark_to_tracking_transform_;
|
||||
const double translation_weight_;
|
||||
const double rotation_weight_;
|
||||
const double interpolation_parameter_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
||||
|
|
@ -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.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/pose_graph/landmark_cost_function.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
#include "gmock/gmock.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
using ::testing::DoubleEq;
|
||||
using ::testing::ElementsAre;
|
||||
|
||||
using LandmarkObservation =
|
||||
mapping::PoseGraph::LandmarkNode::LandmarkObservation;
|
||||
|
||||
TEST(LandmarkCostFunctionTest, SmokeTest) {
|
||||
auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction(
|
||||
LandmarkObservation{
|
||||
0 /* trajectory ID */,
|
||||
common::FromUniversal(5) /* time */,
|
||||
transform::Rigid3d::Translation(Eigen::Vector3d(1., 1., 1.)),
|
||||
1. /* translation_weight */,
|
||||
2. /* rotation_weight */,
|
||||
},
|
||||
common::FromUniversal(0), common::FromUniversal(10));
|
||||
|
||||
std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
|
||||
std::array<double, 3> prev_node_translation{{0., 0., 0.}};
|
||||
std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}};
|
||||
std::array<double, 3> next_node_translation{{2., 2., 2.}};
|
||||
std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
|
||||
std::array<double, 3> landmark_translation{{1., 2., 2.}};
|
||||
const double* parameter_blocks[] = {
|
||||
prev_node_rotation.data(), prev_node_translation.data(),
|
||||
next_node_rotation.data(), next_node_translation.data(),
|
||||
landmark_rotation.data(), landmark_translation.data()};
|
||||
|
||||
std::array<double, 6> residuals;
|
||||
cost_function->Evaluate(parameter_blocks, residuals.data(), nullptr);
|
||||
|
||||
EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
|
||||
DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
Loading…
Reference in New Issue