transform_wrapper.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/
#pragma once

#include <deque>
#include <memory>
#include <string>

#include "Eigen/Core"
#include "Eigen/Dense"

#include "modules/transform/buffer.h"

namespace apollo {
namespace perception {
namespace onboard {

using apollo::transform::Buffer;

DECLARE_string(obs_sensor2novatel_tf2_frame_id);
DECLARE_string(obs_novatel2world_tf2_frame_id);
DECLARE_string(obs_novatel2world_tf2_child_frame_id);
DECLARE_double(obs_tf2_buff_size);

struct StampedTransform {
  double timestamp = 0.0;  // in second
  Eigen::Translation3d translation;
  Eigen::Quaterniond rotation;
};

class TransformCache {
 public:
  TransformCache() = default;
  ~TransformCache() = default;

  void AddTransform(const StampedTransform& transform);
  bool QueryTransform(double timestamp, StampedTransform* transform,
                      double max_duration = 0.0);

  inline void SetCacheDuration(double duration) { cache_duration_ = duration; }

 protected:
  // in ascending order of time
  std::deque<StampedTransform> transforms_;
  double cache_duration_ = 1.0;
};

class TransformWrapper {
 public:
  TransformWrapper() {}
  ~TransformWrapper() = default;

  void Init(const std::string& sensor2novatel_tf2_child_frame_id);
  void Init(const std::string& sensor2novatel_tf2_frame_id,
            const std::string& sensor2novatel_tf2_child_frame_id,
            const std::string& novatel2world_tf2_frame_id,
            const std::string& novatel2world_tf2_child_frame_id);

  // Attention: must initialize TransformWrapper first
  bool GetSensor2worldTrans(double timestamp,
                            Eigen::Affine3d* sensor2world_trans,
                            Eigen::Affine3d* novatel2world_trans = nullptr);

  bool GetExtrinsics(Eigen::Affine3d* trans);

  // Attention: can be called without initlization
  bool GetTrans(double timestamp, Eigen::Affine3d* trans,
                const std::string& frame_id, const std::string& child_frame_id);

  bool GetExtrinsicsBySensorId(const std::string& from_sensor_id,
                               const std::string& to_sensor_id,
                               Eigen::Affine3d* trans);

 protected:
  bool QueryTrans(double timestamp, StampedTransform* trans,
                  const std::string& frame_id,
                  const std::string& child_frame_id);

 private:
  bool inited_ = false;

  Buffer* tf2_buffer_ = Buffer::Instance();

  std::string sensor2novatel_tf2_frame_id_;
  std::string sensor2novatel_tf2_child_frame_id_;
  std::string novatel2world_tf2_frame_id_;
  std::string novatel2world_tf2_child_frame_id_;

  std::unique_ptr<Eigen::Affine3d> sensor2novatel_extrinsics_;

  TransformCache transform_cache_;
};

}  // namespace onboard
}  // namespace perception
}  // namespace apollo