Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/optimization/ceres_pose.h"
00018
00019 namespace cartographer {
00020 namespace mapping {
00021 namespace optimization {
00022
00023 CeresPose::Data FromPose(const transform::Rigid3d& pose) {
00024 return CeresPose::Data{{{pose.translation().x(), pose.translation().y(),
00025 pose.translation().z()}},
00026 {{pose.rotation().w(), pose.rotation().x(),
00027 pose.rotation().y(), pose.rotation().z()}}};
00028 }
00029
00030 CeresPose::CeresPose(
00031 const transform::Rigid3d& pose,
00032 std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
00033 std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
00034 ceres::Problem* problem)
00035 : data_(std::make_shared<CeresPose::Data>(FromPose(pose))) {
00036 problem->AddParameterBlock(data_->translation.data(), 3,
00037 translation_parametrization.release());
00038 problem->AddParameterBlock(data_->rotation.data(), 4,
00039 rotation_parametrization.release());
00040 }
00041
00042 const transform::Rigid3d CeresPose::ToRigid() const {
00043 return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
00044 }
00045
00046 }
00047 }
00048 }