diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function.h b/cartographer/mapping_2d/pose_graph/landmark_cost_function.h new file mode 100644 index 0000000..a746cd7 --- /dev/null +++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function.h @@ -0,0 +1,129 @@ +/* + * 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_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping/pose_graph/cost_helpers.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping_2d { +namespace pose_graph { + +// Cost function measuring the weighted error between the observed pose given by +// the landmark measurement and the linearly interpolated pose of embedded in 3D +// space node poses. +class LandmarkCostFunction { + public: + using LandmarkObservation = + mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; + + static ceres::CostFunction* CreateAutoDiffCostFunction( + const LandmarkObservation& observation, const NodeData& prev_node, + const NodeData& next_node) { + return new ceres::AutoDiffCostFunction< + LandmarkCostFunction, 3 /* residuals */, + 3 /* previous node pose variables */, 3 /* next node pose variables */, + 4 /* landmark rotation variables */, + 3 /* landmark translation variables */>( + new LandmarkCostFunction(observation, prev_node, next_node)); + } + + template + bool operator()(const T* const prev_node_pose, const T* const next_node_pose, + const T* const landmark_rotation, + const T* const landmark_translation, T* const e) const { + using mapping::pose_graph::ComputeUnscaledError; + using mapping::pose_graph::ScaleError; + using mapping::pose_graph::SlerpQuaternions; + + const std::array interpolated_pose_translation{ + {prev_node_pose[0] + + interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]), + prev_node_pose[1] + + interpolation_parameter_ * (next_node_pose[1] - prev_node_pose[1]), + T(0)}}; + + // The following is equivalent to (Embed3D(prev_pose) * + // Rigid3d::Rotation(prev_pose_gravity_alignment)).rotation(). + const Eigen::Quaternion prev_quaternion( + (Eigen::AngleAxis(prev_node_pose[2], + Eigen::Matrix::UnitZ()) * + prev_node_gravity_alignment_.cast()) + .normalized()); + const std::array prev_node_rotation = { + {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(), + prev_quaternion.z()}}; + + // The following is equivalent to (Embed3D(next_pose) * + // Rigid3d::Rotation(next_pose_gravity_alignment)).rotation(). + const Eigen::Quaternion next_quaternion( + (Eigen::AngleAxis(next_node_pose[2], + Eigen::Matrix::UnitZ()) * + next_node_gravity_alignment_.cast()) + .normalized()); + const std::array next_node_rotation = { + {next_quaternion.w(), next_quaternion.x(), next_quaternion.y(), + next_quaternion.z()}}; + + const std::array interpolated_pose_rotation = + SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(), + T(interpolation_parameter_)); + + const std::array error = ScaleError( + ComputeUnscaledError(landmark_to_tracking_transform_, + interpolated_pose_rotation.data(), + interpolated_pose_translation.data(), + landmark_rotation, landmark_translation), + T(translation_weight_), T(rotation_weight_)); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + LandmarkCostFunction(const LandmarkObservation& observation, + const NodeData& prev_node, const NodeData& next_node) + : landmark_to_tracking_transform_( + observation.landmark_to_tracking_transform), + prev_node_gravity_alignment_(prev_node.gravity_alignment), + next_node_gravity_alignment_(next_node.gravity_alignment), + 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 Eigen::Quaterniond prev_node_gravity_alignment_; + const Eigen::Quaterniond next_node_gravity_alignment_; + const double translation_weight_; + const double rotation_weight_; + const double interpolation_parameter_; +}; + +} // namespace pose_graph +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc new file mode 100644 index 0000000..7a47126 --- /dev/null +++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc @@ -0,0 +1,70 @@ +/* + * 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_2d/pose_graph/landmark_cost_function.h" +#include "cartographer/transform/rigid_transform.h" + +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace pose_graph { +namespace { + +using ::testing::DoubleEq; +using ::testing::ElementsAre; + +using LandmarkObservation = + mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; + +TEST(LandmarkCostFunctionTest, SmokeTest) { + NodeData prev_node; + prev_node.time = common::FromUniversal(0); + prev_node.gravity_alignment = Eigen::Quaterniond::Identity(); + NodeData next_node; + next_node.time = common::FromUniversal(10); + next_node.gravity_alignment = Eigen::Quaterniond::Identity(); + + 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 */, + }, + prev_node, next_node); + + std::array prev_node_pose{{2., 0., 0.}}; + std::array next_node_pose{{0., 2., 0.}}; + std::array landmark_rotation{{1., 0., 0., 0.}}; + std::array landmark_translation{{1., 2., 1.}}; + const double* parameter_blocks[] = { + prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(), + landmark_translation.data()}; + + std::array 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_2d +} // namespace cartographer