camera_ground_plane.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 <vector>

#include "cyber/common/log.h"
#include "modules/perception/common/i_lib/core/i_blas.h"
#include "modules/perception/common/i_lib/geometry/i_line.h"

namespace apollo {
namespace perception {
namespace camera {

// ground conversion utils
void ConvertGround3ToGround4(const float &baseline,
                             const std::vector<float> &k_mat,
                             const std::vector<float> &ground3,
                             std::vector<float> *ground4);

bool ConvertGround4ToGround3(const float &baseline,
                             const std::vector<float> &k_mat,
                             const std::vector<float> &ground4,
                             std::vector<float> *ground3);

void GetGroundPlanePitchHeight(const float &baseline,
                               const std::vector<float> &k_mat,
                               const std::vector<float> &ground3, float *pitch,
                               float *cam_height);

void GetGround3FromPitchHeight(const std::vector<float> &k_mat,
                               const float &baseline, const float &pitch,
                               const float &cam_height,
                               std::vector<float> *ground3);

// @brief: a ground plane tracker using pitch + camera height representation.
// @param [in]: per-frame p(pitch)h(height) and inlier ratio
// @param [in/out]: smoothed plane
class GroundPlaneTracker {
 public:
  explicit GroundPlaneTracker(int track_length);
  ~GroundPlaneTracker() {}

  void Push(const std::vector<float> &ph, const float &inlier_ratio);

  void GetGround(float *pitch, float *cam_height);

  void Restart();

  int GetCurTrackLength() {
    int length = static_cast<int>(pitch_height_inlier_tracks_.size());
    CHECK_GE(length, 0);
    return length;
  }

 private:
  int head_ = 0;
  std::vector<float> pitch_height_inlier_tracks_;  // pitch, height, inlier ...
  std::vector<float> const_weight_temporal_;
  std::vector<float> weight_;
};

// params for ground detector
struct CameraGroundPlaneParams {
  CameraGroundPlaneParams() { SetDefault(); }
  void SetDefault();

  int min_nr_samples;
  int nr_frames_track;
  float max_tilt_angle;
  float max_camera_ground_height;
  float min_inlier_ratio;
  float thres_inlier_plane_fitting;

  void operator=(const CameraGroundPlaneParams &params) {
    this->min_nr_samples = params.min_nr_samples;
    this->nr_frames_track = params.nr_frames_track;
    this->max_tilt_angle = params.max_tilt_angle;
    this->min_inlier_ratio = params.min_inlier_ratio;
    this->thres_inlier_plane_fitting = params.thres_inlier_plane_fitting;
  }
};

// @brief: a ground plane detector using disparity.
// @param [in]: disparity samples (row-disp pairs)
// @param [in/out]: ground plane (a * y + b * disp + c = 0)
class CameraGroundPlaneDetector {
 public:
  CameraGroundPlaneDetector() {
    ground_plane_tracker_ = new GroundPlaneTracker(params_.nr_frames_track);
  }
  ~CameraGroundPlaneDetector() {
    delete ground_plane_tracker_;
    ground_plane_tracker_ = nullptr;
  }

  void Init(const std::vector<float> &k_mat, int width, int height,
            float baseline, int max_nr_samples = 1080) {
    CHECK_EQ(k_mat.size(), 9U);
    memcpy(k_mat_, k_mat.data(), sizeof(float) * 9);
    width_ = width;
    height_ = height;
    baseline_ = baseline;
    ss_flt_.resize(max_nr_samples * 2);
    ss_int_.resize(max_nr_samples * 2);
  }

  int get_min_nr_samples() { return params_.min_nr_samples; }

  // main interface: fit the plane from v-d samples
  // input a backup (default) pitch + camera_height
  // can be assigned by {a, b, c, d}: a * X + b * Y + c * Z + d = 0 (cam coor)
  bool DetetGround(float pitch, float camera_height,
                   float *vd, /*samples: v0, d0 ... vj, dj*/
                   int count_vd, const std::vector<float> &plane = {});

  bool GetGroundModel(float *l) const {
    l[0] = l_[0];
    l[1] = l_[1];
    l[2] = l_[2];
    return ground_is_valid_;
  }

 private:
  void FillGroundModel(const std::vector<float> &ground3) {
    CHECK_EQ(ground3.size(), 3U);
    l_[0] = ground3[0];
    l_[1] = ground3[1];
    l_[2] = ground3[2];
  }

  bool DetectGroundFromSamples(float *vd, int count_vd, float *inlier_ratio);

 private:
  CameraGroundPlaneParams params_;
  int width_ = 0;
  int height_ = 0;
  float k_mat_[9] = {0};
  float baseline_ = 0.0f;  // if disp is reversed-z, baseline = 1 / focal
  float l_[3] = {0};       // a * y + b * disp + c = 0

  // scratch space
  std::vector<int> ss_int_ = {};
  std::vector<float> ss_flt_ = {};

  bool ground_is_valid_ = false;  // status of ground estimation

  GroundPlaneTracker *ground_plane_tracker_ = nullptr;
};

/*
 fitting utils
*/
template <typename T>
void GroundHypoGenFunc(const T *v, const T *d, T *p) {
  // disp = p0 * y + p1 -> l = {p0, -1, p1}
  T x[2] = {v[0], d[0]};
  T xp[2] = {v[1], d[1]};
  T l[3] = {0};
  common::ILineFit2d(x, xp, l);
  p[0] = -l[0] * common::IRec(l[1]);
  p[1] = -l[2] * common::IRec(l[1]);
}

template <typename T>
void GroundFittingCostFunc(const T *p, const T *v, const T *d, int n,
                           int *nr_inlier,  // NOLINT compatible for i-lib
                           int *inliers,
                           T *cost,  // NOLINT
                           T error_tol) {
  *cost = static_cast<T>(0.0f);
  *nr_inlier = 0;
  const T *refx = v;
  const T *refp = d;
  for (int i = 0; i < n; ++i) {
    T d_proj = refx[0] * p[0] + p[1];
    T proj_err = static_cast<T>(fabs(d_proj - refp[0]));
    if (proj_err < error_tol) {
      inliers[(*nr_inlier)++] = i;
      *cost += proj_err;
    }
    ++refx;
    ++refp;
  }
}

}  // namespace camera
}  // namespace perception
}  // namespace apollo