Exemplos de Matrix3d::coeffRef em C++ (Cpp)

Exemplo n.º 1

Arquivo: centroid.hpp Projeto: 9gel/hellopcl

template <typename PointT> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              const std::vector<int> &indices,
                              Eigen::Matrix3d &covariance_matrix)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();

  unsigned int point_count;
  if (cloud.is_dense)
  {
    point_count = indices.size ();
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      //const PointT& point = cloud[*iIt];
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y;
      accu [4] += cloud[*iIt].y * cloud[*iIt].z;
      accu [5] += cloud[*iIt].z * cloud[*iIt].z;
    }
  }
  else
  {
    point_count = 0;
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      if (!isFinite (cloud[*iIt]))
        continue;

      ++point_count;
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y;
      accu [4] += cloud[*iIt].y * cloud[*iIt].z;
      accu [5] += cloud[*iIt].z * cloud[*iIt].z;
    }
  }
  if (point_count != 0)
  {
    accu /= static_cast<double> (point_count);
    covariance_matrix.coeffRef (0) = accu [0];
    covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
    covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
    covariance_matrix.coeffRef (4) = accu [3];
    covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
    covariance_matrix.coeffRef (8) = accu [5];
  }
  return (point_count);
}

Exemplo n.º 2

Arquivo: centroid.hpp Projeto: 9gel/hellopcl

template <typename PointT> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              Eigen::Matrix3d &covariance_matrix)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();

  unsigned int point_count;
  if (cloud.is_dense)
  {
    point_count = cloud.size ();
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
    }
  }
  else
  {
    point_count = 0;
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      if (!isFinite (cloud[i]))
        continue;

      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      ++point_count;
    }
  }

  if (point_count != 0)
  {
    accu /= static_cast<double> (point_count);
    covariance_matrix.coeffRef (0) = accu [0];
    covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
    covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
    covariance_matrix.coeffRef (4) = accu [3];
    covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
    covariance_matrix.coeffRef (8) = accu [5];
  }
  return (point_count);
}

Exemplo n.º 3

Arquivo: math.cpp Projeto: ChefOtter/ahl_ros_pkg

    void rpyToRotationMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& mat)
    {
      double sin_a = sin(rpy.coeff(2));
      double cos_a = cos(rpy.coeff(2));
      double sin_b = sin(rpy.coeff(1));
      double cos_b = cos(rpy.coeff(1));
      double sin_g = sin(rpy.coeff(0));
      double cos_g = cos(rpy.coeff(0));

      mat.coeffRef(0, 0) = cos_a * cos_b;
      mat.coeffRef(0, 1) = cos_a * sin_b * sin_g - sin_a * cos_g;
      mat.coeffRef(0, 2) = cos_a * sin_b * cos_g + sin_a * sin_g;
      mat.coeffRef(1, 0) = sin_a * cos_b;
      mat.coeffRef(1, 1) = sin_a * sin_b * sin_g + cos_a * cos_g;
      mat.coeffRef(1, 2) = sin_a * sin_b * cos_g - cos_a * sin_g;
      mat.coeffRef(2, 0) = -sin_b;
      mat.coeffRef(2, 1) = cos_b * sin_g;
      mat.coeffRef(2, 2) = cos_b * cos_g;
    }

Exemplo n.º 4

Arquivo: math.cpp Projeto: ChefOtter/ahl_ros_pkg

    void rpyToRotationMatrix(const std::vector<double>& rpy, Eigen::Matrix3d& mat)
    {
      double sin_a = sin(rpy[2]);
      double cos_a = cos(rpy[2]);
      double sin_b = sin(rpy[1]);
      double cos_b = cos(rpy[1]);
      double sin_g = sin(rpy[0]);
      double cos_g = cos(rpy[0]);

      mat.coeffRef(0, 0) = cos_a * cos_b;
      mat.coeffRef(0, 1) = cos_a * sin_b * sin_g - sin_a * cos_g;
      mat.coeffRef(0, 2) = cos_a * sin_b * cos_g + sin_a * sin_g;
      mat.coeffRef(1, 0) = sin_a * cos_b;
      mat.coeffRef(1, 1) = sin_a * sin_b * sin_g + cos_a * cos_g;
      mat.coeffRef(1, 2) = sin_a * sin_b * cos_g - cos_a * sin_g;
      mat.coeffRef(2, 0) = -sin_b;
      mat.coeffRef(2, 1) = cos_b * sin_g;
      mat.coeffRef(2, 2) = cos_b * cos_g;
    }

Exemplo n.º 5

Arquivo: centroid.hpp Projeto: MorS25/pcl-fuerte

template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                const std::vector<int> &indices,
                                Eigen::Matrix3d &covariance_matrix,
                                Eigen::Vector4d &centroid)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
  unsigned point_count;
  if (cloud.is_dense)
  {
    point_count = indices.size ();
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      //const PointT& point = cloud[*iIt];
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y;
      accu [4] += cloud[*iIt].y * cloud[*iIt].z;
      accu [5] += cloud[*iIt].z * cloud[*iIt].z;
      accu [6] += cloud[*iIt].x;
      accu [7] += cloud[*iIt].y;
      accu [8] += cloud[*iIt].z;
    }
  }
  else
  {
    point_count = 0;
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      if (!isFinite (cloud[*iIt]))
        continue;

      ++point_count;
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4
      accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5
      accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8
      accu [6] += cloud[*iIt].x;
      accu [7] += cloud[*iIt].y;
      accu [8] += cloud[*iIt].z;
    }
  }

  if (point_count != 0)
  {
    accu /= (double) point_count;
    Eigen::Vector3f vec = accu.tail<3> ();
    centroid.head<3> () = vec;//= accu.tail<3> ();
    centroid[3] = 0;
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
  }
  return (point_count);
}

Exemplo n.º 6

Arquivo: centroid.hpp Projeto: MorS25/pcl-fuerte

template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                     Eigen::Matrix3d &covariance_matrix,
                                     Eigen::Vector4d &centroid)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
  unsigned int point_count;
  if (cloud.is_dense)
  {
    point_count = cloud.size ();
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      accu [6] += cloud[i].x;
      accu [7] += cloud[i].y;
      accu [8] += cloud[i].z;
    }
  }
  else
  {
    point_count = 0;
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      if (!isFinite (cloud[i]))
        continue;

      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      accu [6] += cloud[i].x;
      accu [7] += cloud[i].y;
      accu [8] += cloud[i].z;
      ++point_count;
    }
  }

  if (point_count != 0)
  {
    accu /= (double) point_count;
    centroid.head<3> () = accu.tail<3> ();
    centroid[3] = 0;
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
  }
  return (point_count);
}