116 lines
4.2 KiB
C++
116 lines
4.2 KiB
C++
/**
|
|
BSD 3-Clause License
|
|
|
|
This file is part of the Basalt project.
|
|
https://gitlab.com/VladyslavUsenko/basalt-headers.git
|
|
|
|
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
|
|
* Redistributions of source code must retain the above copyright notice, this
|
|
list of conditions and the following disclaimer.
|
|
|
|
* Redistributions in binary form must reproduce the above copyright notice,
|
|
this list of conditions and the following disclaimer in the documentation
|
|
and/or other materials provided with the distribution.
|
|
|
|
* Neither the name of the copyright holder nor the names of its
|
|
contributors may be used to endorse or promote products derived from
|
|
this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
|
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
@file
|
|
@brief Generic local parametrization for Sophus Lie group types to be used with
|
|
ceres.
|
|
*/
|
|
|
|
/**
|
|
File adapted from Sophus
|
|
|
|
Copyright 2011-2017 Hauke Strasdat
|
|
2012-2017 Steven Lovegrove
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to
|
|
deal in the Software without restriction, including without limitation the
|
|
rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
|
sell copies of the Software, and to permit persons to whom the Software is
|
|
furnished to do so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in
|
|
all copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
IN THE SOFTWARE.
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <ceres/local_parameterization.h>
|
|
#include <sophus/se3.hpp>
|
|
|
|
namespace basalt {
|
|
|
|
/// @brief Local parametrization for ceres that can be used with Sophus Lie
|
|
/// group implementations.
|
|
template <class Groupd>
|
|
class LieLocalParameterization : public ceres::LocalParameterization {
|
|
public:
|
|
virtual ~LieLocalParameterization() {}
|
|
|
|
using Tangentd = typename Groupd::Tangent;
|
|
|
|
/// @brief plus operation for Ceres
|
|
///
|
|
/// T * exp(x)
|
|
///
|
|
virtual bool Plus(double const* T_raw, double const* delta_raw,
|
|
double* T_plus_delta_raw) const {
|
|
Eigen::Map<Groupd const> const T(T_raw);
|
|
Eigen::Map<Tangentd const> const delta(delta_raw);
|
|
Eigen::Map<Groupd> T_plus_delta(T_plus_delta_raw);
|
|
T_plus_delta = T * Groupd::exp(delta);
|
|
return true;
|
|
}
|
|
|
|
///@brief Jacobian of plus operation for Ceres
|
|
///
|
|
/// Dx T * exp(x) with x=0
|
|
///
|
|
virtual bool ComputeJacobian(double const* T_raw,
|
|
double* jacobian_raw) const {
|
|
Eigen::Map<Groupd const> T(T_raw);
|
|
Eigen::Map<Eigen::Matrix<double, Groupd::num_parameters, Groupd::DoF,
|
|
Eigen::RowMajor>>
|
|
jacobian(jacobian_raw);
|
|
jacobian = T.Dx_this_mul_exp_x_at_0();
|
|
return true;
|
|
}
|
|
|
|
///@brief Global size
|
|
virtual int GlobalSize() const { return Groupd::num_parameters; }
|
|
|
|
///@brief Local size
|
|
virtual int LocalSize() const { return Groupd::DoF; }
|
|
};
|
|
|
|
} // namespace basalt
|