similaritytransform.h

Go to the documentation of this file.

/*
 * Copyright (c) 2012, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * You can contact the author at <acmarkus at ethz dot ch>
 *
 * 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 SIMILARITYTRANSFORM_H_
#define SIMILARITYTRANSFORM_H_

#include <vector>
#include <utility>
#include <geometry_msgs/PoseWithCovariance.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>

#include <msf_core/msf_types.hpp>
#include <msf_core/eigen_utils.h>

//  geometry_msgs::PoseWithCovariance.
namespace geometry_msgs {
namespace cov {
enum {
  p = 0,
  q = 3
};
}
}

namespace msf_core {

inline Vector3 geometry_msgsToEigen(const geometry_msgs::Point & p) {
  return (Vector3() << p.x, p.y, p.z).finished();
}

inline Eigen::Quaterniond geometry_msgsToEigen(
    const geometry_msgs::Quaternion & q) {
  return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
}

inline Matrix3 geometry_msgsCovBlockToEigen(
    const geometry_msgs::PoseWithCovariance::_covariance_type & gcov,
    int start_row, int start_col) {
  Eigen::Map<const Matrix6> cov(gcov.data());
  return Matrix3(cov.block<3, 3>(start_row, start_col));
}

template<class Derived>
inline void eigenCovBlockToGeometry_msgs(
    geometry_msgs::PoseWithCovariance::_covariance_type & gcov,
    const Eigen::MatrixBase<Derived> &ecov, int start_row, int start_col) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
  Eigen::Map<Matrix6> _gcov(gcov.data());
  _gcov.block<3, 3>(start_row, start_col) = ecov;
  // Off diagonal entries --> we need to copy it to the opposite off-diagonal
  // as well.
  if (start_row != start_col)
    _gcov.block<3, 3>(start_col, start_row) = ecov.transpose();
}

template<class Derived>
inline geometry_msgs::Point eigenToGeometry_msgs(
    const Eigen::MatrixBase<Derived> & p) {
  EIGEN_STATIC_ASSERT_VECTOR_ONLY (Derived);
  assert(p.size() == 3);

  geometry_msgs::Point _p;
  _p.x = p[0];
  _p.y = p[1];
  _p.z = p[2];
  return _p;
}

inline geometry_msgs::Quaternion eigenToGeometry_msgs(
    const Eigen::Quaterniond & q) {
  geometry_msgs::Quaternion _q;
  _q.w = q.w();
  _q.x = q.x();
  _q.y = q.y();
  _q.z = q.z();
  return _q;
}

namespace similarity_transform {

typedef geometry_msgs::PoseWithCovariance Pose;
typedef std::pair<Pose, Pose> PosePair;
typedef std::vector<PosePair> PosePairVector;

class From6DoF {
 public:
  From6DoF();
  void addMeasurement(const PosePair & measurement);

  void addMeasurement(const Pose & pose1, const Pose & pose2);

  bool compute(Pose & pose, double *scale = nullptr , double *cond = nullptr ,
               double eps = std::numeric_limits<double>::epsilon() * 4 * 4);
 private:
  PosePairVector measurements_;
};
}
}  // namespace msf_core
#endif  // SIMILARITYTRANSFORM_H_