17 #ifndef COST_FUNCTOR_H 18 #define COST_FUNCTOR_H 24 #include "Eigen/Geometry" 25 #include "ceres/ceres.h" 27 template <
typename FloatType>
30 using Affine = Eigen::Transform<FloatType, 3, Eigen::Affine>;
31 using Vector = Eigen::Matrix<FloatType, 3, 1>;
50 return Rigid3(Vector::Zero(), rotation);
54 return Rigid3(vector, Quaternion::Identity());
61 template <
typename OtherType>
73 return Rigid3(translation, rotation);
84 out.append(
"], q: [");
85 out.append(std::to_string(
rotation().
w()));
87 out.append(std::to_string(
rotation().
x()));
89 out.append(std::to_string(
rotation().
y()));
91 out.append(std::to_string(
rotation().
z()));
101 template <
typename FloatType>
109 template <
typename FloatType>
117 template <
typename T>
132 Eigen::Matrix<double, 2, 1> pos_gps,
134 : pos_world_(pos_world),
136 inv_covariance_(1.0/covariance){
137 if(covariance < 1e-7)
139 LOG(WARNING)<<
"TransformDeltaCostFunctor covariance too small: "<<covariance;
140 inv_covariance_ = 1.0;
147 template <
typename T>
149 T*
const residual)
const {
151 Eigen::Matrix<T, 3, 1>(translation[0], translation[1], T(0)),
152 Eigen::Quaternion<T>(ceres::cos(rotation[0]/T(2)), T(0), T(0),
153 ceres::sin(rotation[0]/T(2))));
154 return Evaluate(transform, residual);
157 template <
typename T>
159 T*
const residual)
const {
160 const Eigen::Matrix<T, 3, 1> delta =
161 Eigen::Matrix<T, 3, 1>(T(pos_gps_[0]), T(pos_gps_[1]), T(0))
162 - transform * Eigen::Matrix<T, 3, 1>(T(pos_world_[0]), T(pos_world_[1]), T(0));
163 residual[0] = T(inv_covariance_)*delta[0];
164 residual[1] = T(inv_covariance_)*delta[1];
177 #endif // COST_FUNCTOR_H Rigid3(const Affine &affine)
const Quaternion & rotation() const
Rigid3(const Vector &translation, const Quaternion &rotation)
static Rigid3 Rotation(const AngleAxis &angle_axis)
Eigen::Quaternion< FloatType > Quaternion
Rigid3(const Vector &translation, const AngleAxis &rotation)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Eigen::Matrix< FloatType, 3, 1 > Vector
TFSIMD_FORCE_INLINE Vector3 normalized() const
static Rigid3< FloatType > Identity()
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::string DebugString() const
const Vector & translation() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
Rigid3< OtherType > cast() const
static Rigid3 Translation(const Vector &vector)
static Rigid3 Rotation(const Quaternion &rotation)
Eigen::AngleAxis< FloatType > AngleAxis
Eigen::Transform< FloatType, 3, Eigen::Affine > Affine