transform_delta_cost_functor.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef COST_FUNCTOR_H
00018 #define COST_FUNCTOR_H
00019 
00020 #include <iostream>
00021 #include <string>
00022 
00023 #include "Eigen/Core"
00024 #include "Eigen/Geometry"
00025 #include "ceres/ceres.h"
00026 
00027 template <typename FloatType>
00028 class Rigid3 {
00029 public:
00030     using Affine = Eigen::Transform<FloatType, 3, Eigen::Affine>;
00031     using Vector = Eigen::Matrix<FloatType, 3, 1>;
00032     using Quaternion = Eigen::Quaternion<FloatType>;
00033     using AngleAxis = Eigen::AngleAxis<FloatType>;
00034 
00035     Rigid3()
00036         : translation_(Vector::Identity()), rotation_(Quaternion::Identity()) {}
00037     // TODO(damonkohler): Remove
00038     explicit Rigid3(const Affine& affine)
00039         : translation_(affine.translation()), rotation_(affine.rotation()) {}
00040     Rigid3(const Vector& translation, const Quaternion& rotation)
00041         : translation_(translation), rotation_(rotation) {}
00042     Rigid3(const Vector& translation, const AngleAxis& rotation)
00043         : translation_(translation), rotation_(rotation) {}
00044 
00045     static Rigid3 Rotation(const AngleAxis& angle_axis) {
00046         return Rigid3(Vector::Zero(), Quaternion(angle_axis));
00047     }
00048 
00049     static Rigid3 Rotation(const Quaternion& rotation) {
00050         return Rigid3(Vector::Zero(), rotation);
00051     }
00052 
00053     static Rigid3 Translation(const Vector& vector) {
00054         return Rigid3(vector, Quaternion::Identity());
00055     }
00056 
00057     static Rigid3<FloatType> Identity() {
00058         return Rigid3<FloatType>(Vector::Zero(), Quaternion::Identity());
00059     }
00060 
00061     template <typename OtherType>
00062     Rigid3<OtherType> cast() const {
00063         return Rigid3<OtherType>(translation_.template cast<OtherType>(),
00064                                  rotation_.template cast<OtherType>());
00065     }
00066 
00067     const Vector& translation() const { return translation_; }
00068     const Quaternion& rotation() const { return rotation_; }
00069 
00070     Rigid3 inverse() const {
00071         const Quaternion rotation = rotation_.conjugate();
00072         const Vector translation = -(rotation * translation_);
00073         return Rigid3(translation, rotation);
00074     }
00075 
00076     std::string DebugString() const {
00077         std::string out;
00078         out.append("{ t: [");
00079         out.append(std::to_string(translation().x()));
00080         out.append(", ");
00081         out.append(std::to_string(translation().y()));
00082         out.append(", ");
00083         out.append(std::to_string(translation().z()));
00084         out.append("], q: [");
00085         out.append(std::to_string(rotation().w()));
00086         out.append(", ");
00087         out.append(std::to_string(rotation().x()));
00088         out.append(", ");
00089         out.append(std::to_string(rotation().y()));
00090         out.append(", ");
00091         out.append(std::to_string(rotation().z()));
00092         out.append("] }");
00093         return out;
00094     }
00095 
00096 private:
00097     Vector translation_;
00098     Quaternion rotation_;
00099 };
00100 
00101 template <typename FloatType>
00102 Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
00103                             const Rigid3<FloatType>& rhs) {
00104     return Rigid3<FloatType>(
00105                 lhs.rotation() * rhs.translation() + lhs.translation(),
00106                 (lhs.rotation() * rhs.rotation()).normalized());
00107 }
00108 
00109 template <typename FloatType>
00110 typename Rigid3<FloatType>::Vector operator*(
00111         const Rigid3<FloatType>& rigid,
00112         const typename Rigid3<FloatType>::Vector& point) {
00113     return rigid.rotation() * point + rigid.translation();
00114 }
00115 
00116 // This is needed for gmock.
00117 template <typename T>
00118 std::ostream& operator<<(std::ostream& os,
00119                          Rigid3<T>& rigid) {
00120     os << rigid.DebugString();
00121     return os;
00122 }
00123 
00124 using Rigid3d = Rigid3<double>;
00125 using Rigid3f = Rigid3<float>;
00126 
00127 class TransformDeltaCostFunctor {
00128 public:
00129     // Creates an OccupiedSpaceCostFunctor using the specified grid, 'rotation' to
00130     // add to all poses, and point cloud.
00131     TransformDeltaCostFunctor(Eigen::Matrix<double, 2, 1> pos_world,
00132                               Eigen::Matrix<double, 2, 1> pos_gps,
00133                               double covariance)
00134         : pos_world_(pos_world),
00135           pos_gps_(pos_gps),
00136           inv_covariance_(1.0/covariance){
00137         if(covariance < 1e-7)
00138         {
00139             LOG(WARNING)<<"TransformDeltaCostFunctor covariance too small: "<<covariance;
00140             inv_covariance_ = 1.0;
00141         }
00142     }
00143 
00144     TransformDeltaCostFunctor(const TransformDeltaCostFunctor&) = delete;
00145     TransformDeltaCostFunctor& operator=(const TransformDeltaCostFunctor&) = delete;
00146 
00147     template <typename T>
00148     bool operator()(const T* const translation, const T* const rotation,
00149                     T* const residual) const {
00150         const Rigid3<T> transform(
00151                     Eigen::Matrix<T, 3, 1>(translation[0], translation[1], T(0)),
00152                     Eigen::Quaternion<T>(ceres::cos(rotation[0]/T(2)), T(0), T(0),
00153                 ceres::sin(rotation[0]/T(2))));
00154         return Evaluate(transform, residual);
00155     }
00156 
00157     template <typename T>
00158     bool Evaluate(const Rigid3<T>& transform,
00159                   T* const residual) const {
00160         const Eigen::Matrix<T, 3, 1> delta =
00161                 Eigen::Matrix<T, 3, 1>(T(pos_gps_[0]), T(pos_gps_[1]), T(0))
00162                 - transform * Eigen::Matrix<T, 3, 1>(T(pos_world_[0]), T(pos_world_[1]), T(0));
00163         residual[0] = T(inv_covariance_)*delta[0];
00164         residual[1] = T(inv_covariance_)*delta[1];
00165         return true;
00166     }
00167 
00168 private:
00169     Eigen::Matrix<double, 2, 1> pos_world_;
00170     Eigen::Matrix<double, 2, 1> pos_gps_;
00171     double inv_covariance_;
00172 };
00173 
00174 
00175 
00176 
00177 #endif  // COST_FUNCTOR_H


hector_gps_calibration
Author(s):
autogenerated on Wed Sep 6 2017 02:41:37