ぬの部屋（仮）
nu-no-he-ya

スポンサーリンク

<<前の記事次の記事>> | キーワード：C/C++

OBBを計算する（プログラム）

2020年9月2日公開
2020年9月2日更新

プログラム本体

#pragma once
#include <vector>
#include <array>
#include <limits>
#include <valarray>

template<typename real_t>
real_t Length3(const real_t * const vec) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return sqrt(vec[X] * vec[X] + vec[Y] * vec[Y] + vec[Z] * vec[Z]);
}
template<typename real_t>
bool Normalize3(real_t* const pV)
{

  const int X = 0;
  const int Y = 1;
  const int Z = 2;

  real_t len;
  real_t& x = pV[X];
  real_t& y = pV[Y];
  real_t& z = pV[Z];

  len = static_cast<real_t>(sqrt(x * x + y * y + z * z));

  if (len < static_cast<real_t>(1e-6)) return false;

  len = static_cast<real_t>(1.0) / len;
  x *= len;
  y *= len;
  z *= len;

  return true;
}

template<typename real_t>
real_t Inner3(real_t const* const vec1, real_t const* const vec2) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return ((vec1[X]) * (vec2[X]) + (vec1[Y]) * (vec2[Y]) + (vec1[Z]) * (vec2[Z]));
}

/*!
 * Jacobi法による固有値の算出
 * @param[inout] a 実対称行列．計算後，対角要素に固有値が入る
 * @param[out] v 固有ベクトル(aと同じサイズ)
 * @param[in] n 行列のサイズ(n×n)
 * @param[in] eps 収束誤差
 * @param[in] iter_max 最大反復回数
 * @return 反復回数
 * @see http://www.slis.tsukuba.ac.jp/~fujisawa.makoto.fu/cgi-bin/wiki/index.php?%B8%C7%CD%AD%C3%CD/%B8%C7%CD%AD%A5%D9%A5%AF%A5%C8%A5%EB
 */
template<typename real_t>
int EigenJacobiMethod(real_t* a, real_t* v, int n, real_t eps = 1e-8, int iter_max = 100)
{
  real_t bii, bij, bjj, bji;

  auto bim = std::vector<real_t>(n);
  auto bjm = std::vector<real_t>(n);

  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < n; ++j) {
      v[i * n + j] = (i == j) ? 1.0 : 0.0;
    }
  }

  int cnt = 0;
  for (;;) {
    int i, j;

    float x = 0.0;
    for (int ia = 0; ia < n; ++ia) {
      for (int ja = 0; ja < n; ++ja) {
        int idx = ia * n + ja;
        if (ia != ja && fabs(a[idx]) > x) {
          i = ia;
          j = ja;
          x = std::abs(a[idx]);
        }
      }
    }

    float aii = a[i * n + i];
    float ajj = a[j * n + j];
    float aij = a[i * n + j];

    float alpha, beta;
    alpha = (aii - ajj) / 2.0;
    beta = sqrt(alpha * alpha + aij * aij);

    float st, ct;
    ct = sqrt((1.0 + fabs(alpha) / beta) / 2.0);    // sinθ
    st = (((aii - ajj) >= 0.0) ? 1.0 : -1.0) * aij / (2.0 * beta * ct);    // cosθ

    // A = PAPの計算
    for (int m = 0; m < n; ++m) {
      if (m == i || m == j) continue;

      float aim = a[i * n + m];
      float ajm = a[j * n + m];

      bim[m] = aim * ct + ajm * st;
      bjm[m] = -aim * st + ajm * ct;
    }

    bii = aii * ct * ct + 2.0 * aij * ct * st + ajj * st * st;
    bij = 0.0;

    bjj = aii * st * st - 2.0 * aij * ct * st + ajj * ct * ct;
    bji = 0.0;

    for (int m = 0; m < n; ++m) {
      a[i * n + m] = a[m * n + i] = bim[m];
      a[j * n + m] = a[m * n + j] = bjm[m];
    }
    a[i * n + i] = bii;
    a[i * n + j] = bij;
    a[j * n + j] = bjj;
    a[j * n + i] = bji;

    // V = PVの計算
    for (int m = 0; m < n; ++m) {
      float vmi = v[m * n + i];
      float vmj = v[m * n + j];

      bim[m] = vmi * ct + vmj * st;
      bjm[m] = -vmi * st + vmj * ct;
    }
    for (int m = 0; m < n; ++m) {
      v[m * n + i] = bim[m];
      v[m * n + j] = bjm[m];
    }

    float e = 0.0;
    for (int ja = 0; ja < n; ++ja) {
      for (int ia = 0; ia < n; ++ia) {
        if (ia != ja) {
          e += fabs(a[ja * n + ia]);
        }
      }
    }
    if (e < eps) break;

    cnt++;
    if (cnt > iter_max) break;
  }

  return cnt;
}

//! @brief 固有値・固有ベクトル
struct _eigen_ {
  double _value;   //!< 固有値
  double _vector[3];//!< 固有ベクトル
};


//! @brief データの平均を求める
//! @param [in] vec データの配列
//! @return 各要素の平均
template<typename real_t,typename PointT>
void Covariance_Ave(real_t* ave,const std::vector<PointT>& vec) {

  // 初期化
  ave[0] = 0;
  ave[1] = 0;
  ave[2] = 0;

  // 各要素平均
  for (size_t i = 0; i < vec.size(); i++) {
    ave[0] += vec[i][0];
    ave[1] += vec[i][1];
    ave[2] += vec[i][2];
  }
  ave[0] /= vec.size();
  ave[1] /= vec.size();
  ave[2] /= vec.size();

}

//! @brief 共分散を求める
//! @param [in] average 各要素の平均 3次元
//! @param [in] vec データ
//! @param [in] param どの要素に対して求めるか。例えばxyzの時、x,yに対する共分散なら{0,1}を与える。
//! @return 偏差の積の和の要素数分の一
template<typename real_t,typename PointT>
double Covariance(const real_t* const average, const std::vector<PointT>& vec, const std::array<int, 2>& param) {

  double sum = 0.0;
  for (size_t i = 0; i < vec.size(); i++) {

    //指定したパラメータの偏差を求める
    double deviation[3];
    for (size_t j = 0; j < param.size(); j++) {
      int target = param[j];
      deviation[target] = (vec[i][target] - average[target]);
    }

    //偏差の積
    double product = 1.0;
    for (size_t j = 0; j < param.size(); j++) {
      int target = param[j];
      product *= deviation[target];
    }

    //偏差の積の和を更新
    sum += product;
  }

  //偏差の積の和のN分の一
  return 1.0 / vec.size() * sum;
}


//! @brief データを主成分分析する
//! @param [out] average 全データの各要素の平均
//! @param [out] ev 固有値と固有ベクトル
//! @param [in] pt 三次元の座標値一覧
//! @return なし
template<typename PointT>
void PrincipalComponentAnalysis3D(_eigen_* e, const std::vector<PointT>& pt) {

  double average[3];

  //各要素の平均を求める。
  //これは共分散を求めるときに (x[i] - xの平均)×(y[i] - yの平均) 等の計算が必要なため
  Covariance_Ave(average, pt);

  //共分散を求める
  //第三引数の{0,0}はxxを表す。xyなら{0,1}。これはデータがxyzの順に並んでいる事が前提。
  double Sxx = static_cast<double>(Covariance(average, pt, { 0, 0 }));
  double Sxy = static_cast<double>(Covariance(average, pt, { 0, 1 }));
  double Sxz = static_cast<double>(Covariance(average, pt, { 0, 2 }));
  double Syy = static_cast<double>(Covariance(average, pt, { 1, 1 }));
  double Syz = static_cast<double>(Covariance(average, pt, { 1, 2 }));
  double Szz = static_cast<double>(Covariance(average, pt, { 2, 2 }));

  // 分散共分散行列
  double a[3 * 3] = {
     Sxx,Sxy,Sxz,
     Sxy,Syy,Syz,
     Sxz,Syz,Szz
  };
  double eigen[3 * 3];
  EigenJacobiMethod(a, eigen, 3);

  // eigenの対角線が固有値となっている

  e[0]._value = eigen[0];
  e[0]._vector[0] = eigen[0];
  e[0]._vector[1] = eigen[3];
  e[0]._vector[2] = eigen[6];

  e[1]._value = eigen[4];
  e[1]._vector[0] = eigen[1];
  e[1]._vector[1] = eigen[4];
  e[1]._vector[2] = eigen[7];

  e[2]._value = eigen[8];
  e[2]._vector[0] = eigen[2];
  e[2]._vector[1] = eigen[5];
  e[2]._vector[2] = eigen[8];
}

// OBBを表現する構造体
struct OBBObject {
  double corner[3]; //OBBの角
  double vectorU[3]; //各辺の方向と長さ
  double vectorV[3];
  double vectorN[3];
};

inline std::valarray<double> shift_centersUV(
  const std::valarray<double>& centerU,
  const std::valarray<double>& centerV,
  const std::valarray<double>& centerN,
  const std::valarray<double>& vectorU,
  const std::valarray<double>& vectorV,
  const std::valarray<double>& vectorN
) {

  //////////////////////////////////////////
  // vector vectorU→centerV
  std::valarray<double> vRG = centerV - centerU;

  double Rlen = Inner3(&vectorU[0], &vRG[0]);
  Rlen /= Length3(&vectorU[0]);
  std::valarray<double> direction_uv = vectorU;
  Normalize3(&direction_uv[0]);
  direction_uv *= -Rlen;

  glLineWidth(2);
  glColor3d(1, 0, 0);
  //////////////////////////////////////////
  // vector centerV→centerU
  std::valarray<double> vGR = centerU - centerV;

  double Glen = Inner3(&centerV[0], &vGR[0]);
  Glen /= Length3(&centerV[0]);
  std::valarray<double> direction_vc = centerV;
  Normalize3(&direction_vc[0]);
  direction_vc *= -Glen;

  //////////////////////////////////////////
  return direction_uv + direction_vc;
}

inline std::valarray<double> get_a_corner(
  const std::valarray<double>& centerU,
  const std::valarray<double>& centerV,
  const std::valarray<double>& centerN,
  const std::valarray<double>& vectorU,
  const std::valarray<double>& vectorV,
  const std::valarray<double>& vectorN
) {

  std::valarray<double> ret;

  /////////////////////////////
  // OBBの中央を見つける
  /////////////////////////////
  std::valarray<double> trueCenter
    = centerN + shift_centersUV(
      centerU,
      centerV,
      centerN,
      vectorU,
      vectorV,
      vectorN
    );

  ///////////////////////////////////////
  // OBBの中央から各辺の半分ずつ移動する
  ///////////////////////////////////////

  ret = trueCenter - (vectorU/2.0) - (vectorV/2.0) - (vectorN/2.0);

  return ret;

}

//! @brief 点群を囲むOBBを求める
//! @param [in] obb BoundingBoxを表現する構造体
//! @param [in] pt 計算対象の頂点
template<typename PointT>
void CalcOBB(OBBObject* obb, const std::vector<PointT> pt) {

  using scalar_t = double;

  _eigen_ e[3];

  PrincipalComponentAnalysis3D(e, pt);

  //降順ソート
  std::sort(std::begin(e), std::end(e), [](const _eigen_ & e1, const _eigen_ & e2) {
    return e1._value > e2._value;
  });


  scalar_t obbLen[3];//ボックス辺長さ

  std::array<std::valarray<double>, 3> centerd;
  centerd[0] = std::valarray<double>(3);
  centerd[1] = std::valarray<double>(3);
  centerd[2] = std::valarray<double>(3);
  for (int k = 0; k < 3; k++) {

    scalar_t inner_k_min = (std::numeric_limits<scalar_t>::max)();
    scalar_t inner_k_max = std::numeric_limits<scalar_t>::lowest();

    //第 k+1 主成分とすべての頂点の内積のうち最大・最小を取り出す
    for (size_t i = 0; i < pt.size(); i++) {
      scalar_t p[3] = {
        pt[i][0],
        pt[i][1],
        pt[i][2]
      };
      inner_k_min = (std::min)(
        Inner3(p, e[k]._vector),
        inner_k_min
        );
      inner_k_max = (std::max)(
        Inner3(p, e[k]._vector),
        inner_k_max
        );
    }

    obbLen[k] = (inner_k_max - inner_k_min);


    scalar_t tmp = (inner_k_max + inner_k_min) / 2.0;
    centerd[k][0] = e[k]._vector[0] * tmp;
    centerd[k][1] = e[k]._vector[1] * tmp;
    centerd[k][2] = e[k]._vector[2] * tmp;
  }


  ///////////////////////////////

  std::valarray<double> vecu = {
    e[0]._vector[0] * obbLen[0],
    e[0]._vector[1] * obbLen[0],
    e[0]._vector[2] * obbLen[0]
  };
  std::valarray<double> vecv = {
    e[1]._vector[0] * obbLen[1],
    e[1]._vector[1] * obbLen[1],
    e[1]._vector[2] * obbLen[1]
  };
  std::valarray<double> vecn = {
    e[2]._vector[0] * obbLen[2],
    e[2]._vector[1] * obbLen[2],
    e[2]._vector[2] * obbLen[2]
  };

  std::valarray<double> ret = get_a_corner(
    centerd[0],
    centerd[1],
    centerd[2],
    vecu,
    vecv,
    vecn
  );

  obb->corner[0] = ret[0];
  obb->corner[1] = ret[1];
  obb->corner[2] = ret[2];
  obb->vectorU[0] = vecu[0];
  obb->vectorU[1] = vecu[1];
  obb->vectorU[2] = vecu[2];
  obb->vectorV[0] = vecv[0];
  obb->vectorV[1] = vecv[1];
  obb->vectorV[2] = vecv[2];
  obb->vectorN[0] = vecn[0];
  obb->vectorN[1] = vecn[1];
  obb->vectorN[2] = vecn[2];


}

呼び出し方

	std::vector<std::vector<double> > points; //点群の読み込み先

	read_xyz(points,"C:\\test\\Suzanne.xyz",' '); //点群読み込み

	CalcOBB(&obb, points);// OBB計算

なお以下はxyzファイル読み込み

//! @brief separator区切りの実数の一覧を読み込む
//! @details xyzフォーマットは正式な規格として存在しないらしいので、ある程度の柔軟性を持たせる
//! @param [out] ret 結果を格納する配列の配列
//! @param [in] filename ファイル名
//! @param [in] separator 区切り文字
//! @return なし
template<typename scalar_t>
void read_xyz(std::vector<std::vector<scalar_t> >& ret,const char* filename, const char separator) {

	std::ifstream ifs(filename);

	std::string str;
	while (std::getline(ifs, str)) {

  std::stringstream line{ str };

  std::string value;

  std::vector<scalar_t> tmp;

  while (getline(line, value, separator)){
  	tmp.push_back(std::atof(value.c_str()));
  }
  ret.push_back(tmp);

	}

}

OBB 表示関数

//! @brief 立方体を描画
//! @param [in] width 立方体の一辺の長さ
//! @return なし
void draw_obb(const OBBObject& obb) {
  double xs = obb.corner[0];
  double ys = obb.corner[1];
  double zs = obb.corner[2];

  glEnable(GL_DEPTH_TEST);
  glFrontFace(GL_CCW);//時計回りが表
  glEnable(GL_CULL_FACE);//カリングを有効にする

  glBegin(GL_LINE_LOOP);
  glColor3d(0.5, 0.5, 1.0);
  glVertex3d(xs, ys, zs);//
  glVertex3d(
    xs + obb.vectorU[0],
    ys + obb.vectorU[1],
    zs + obb.vectorU[2]);
  glVertex3d(
    xs + obb.vectorU[0] + obb.vectorV[0],
    ys + obb.vectorU[1] + obb.vectorV[1],
    zs + obb.vectorU[2] + obb.vectorV[2]);
  glVertex3d(
    xs + obb.vectorV[0], 
    ys + obb.vectorV[1], 
    zs + obb.vectorV[2]);
  glEnd();

  glBegin(GL_LINE_LOOP);
  glColor3d(0.0, 0.0, 1.0);
  glVertex3d(
    xs + obb.vectorN[0] + obb.vectorV[0],
    ys + obb.vectorN[1] + obb.vectorV[1],
    zs + obb.vectorN[2] + obb.vectorV[2]);
  glVertex3d(
    xs + obb.vectorN[0] + obb.vectorU[0] + obb.vectorV[0],
    ys + obb.vectorN[1] + obb.vectorU[1] + obb.vectorV[1],
    zs + obb.vectorN[2] + obb.vectorU[2] + obb.vectorV[2]);
  glVertex3d(
    xs + obb.vectorN[0] + obb.vectorU[0],
    ys + obb.vectorN[1] + obb.vectorU[1],
    zs + obb.vectorN[2] + obb.vectorU[2]);
  glVertex3d(
    xs + obb.vectorN[0], 
    ys + obb.vectorN[1], 
    zs + obb.vectorN[2]);//

  glEnd();
  glBegin(GL_LINE_LOOP);
  glColor3d(1.0, 0.0, 0.0);
  glVertex3d(
    xs + obb.vectorV[0],
    ys + obb.vectorV[1],
    zs + obb.vectorV[2]
  );
  glVertex3d(
    xs + obb.vectorN[0] + obb.vectorV[0],
    ys + obb.vectorN[1] + obb.vectorV[1],
    zs + obb.vectorN[2] + obb.vectorV[2]
  );
  glVertex3d(
    xs + obb.vectorN[0],
    ys + obb.vectorN[1],
    zs + obb.vectorN[2]
  );
  glVertex3d(xs, ys, zs);//
  glEnd();


  glBegin(GL_LINE_LOOP);
  glColor3d(1.0, 0.5, 0.5);
  glVertex3d(
    xs + obb.vectorU[0],
    ys + obb.vectorU[1],
    zs + obb.vectorU[2]);//
  glVertex3d(
    xs + obb.vectorU[0] + obb.vectorN[0],
    ys + obb.vectorU[1] + obb.vectorN[1],
    zs + obb.vectorU[2] + obb.vectorN[2]
  );
  glVertex3d(
    xs + obb.vectorU[0] + obb.vectorN[0] + obb.vectorV[0],
    ys + obb.vectorU[1] + obb.vectorN[1] + obb.vectorV[1],
    zs + obb.vectorU[2] + obb.vectorN[2] + obb.vectorV[2]
  );
  glVertex3d(
    xs + obb.vectorU[0] + obb.vectorV[0],
    ys + obb.vectorU[1] + obb.vectorV[1],
    zs + obb.vectorU[2] + obb.vectorV[2]
  );
  glEnd();


  glLineWidth(2);
  glColor3d(0, 1, 0);
  glBegin(GL_LINE_LOOP);
  glVertex3d(
    xs,
    ys,
    zs);
  glVertex3d(
    xs + obb.vectorN[0],
    ys + obb.vectorN[1],
    zs + obb.vectorN[2]
  );
  glVertex3d(
    xs + obb.vectorN[0] + obb.vectorU[0],
    ys + obb.vectorN[1] + obb.vectorU[1],
    zs + obb.vectorN[2] + obb.vectorU[2]
  );
  glVertex3d(
    xs + obb.vectorU[0],
    ys + obb.vectorU[1],
    zs + obb.vectorU[2]
  );
  glEnd();
  glColor3d(0.5, 1, 0.5);
  glBegin(GL_LINE_LOOP);
  glVertex3d(
    xs + obb.vectorV[0] + obb.vectorU[0],
    ys + obb.vectorV[1] + obb.vectorU[1],
    zs + obb.vectorV[2] + obb.vectorU[2]
  );
  glVertex3d(
    xs + obb.vectorV[0] + obb.vectorN[0] + obb.vectorU[0],
    ys + obb.vectorV[1] + obb.vectorN[1] + obb.vectorU[1],
    zs + obb.vectorV[2] + obb.vectorN[2] + obb.vectorU[2]
  );
  glVertex3d(
    xs + obb.vectorV[0] + obb.vectorN[0],
    ys + obb.vectorV[1] + obb.vectorN[1],
    zs + obb.vectorV[2] + obb.vectorN[2]
  );
  glVertex3d(
    xs + obb.vectorV[0],
    ys + obb.vectorV[1],
    zs + obb.vectorV[2]);
  glEnd();


  glDisable(GL_CULL_FACE);//カリングを無効にする
}

コメントを残すコメントをキャンセル

この記事のトラックバックURL：