C++ Vector3f::array方法代码示例

void WorldDownloadManager::findExtraCubesForBoundingBox(const Eigen::Vector3f& current_cube_min,
  const Eigen::Vector3f& current_cube_max,const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,Vector3fVector& cubes_centers,
  bool& extract_current)
{
  const Eigen::Vector3f & cube_size = current_cube_max - current_cube_min;
  cubes_centers.clear();
  extract_current = false;

  const Eigen::Vector3f relative_act_bbox_min = bbox_min - current_cube_min;
  const Eigen::Vector3f relative_act_bbox_max = bbox_max - current_cube_min;
  const Eigen::Vector3i num_cubes_plus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_max.array() / cube_size.array())
    - (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
  const Eigen::Vector3i num_cubes_minus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_min.array() / cube_size.array())
    + (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
  for (int z = num_cubes_minus.z(); z <= num_cubes_plus.z(); z++)
    for (int y = num_cubes_minus.y(); y <= num_cubes_plus.y(); y++)
      for (int x = num_cubes_minus.x(); x <= num_cubes_plus.x(); x++)
      {
        const Eigen::Vector3i cube_index(x,y,z);
        if ((cube_index.array() == Eigen::Vector3i::Zero().array()).all())
        {
          extract_current = true;
          continue;
        }

        const Eigen::Vector3f relative_cube_origin = cube_index.cast<float>().array() * cube_size.array();
        const Eigen::Vector3f cube_center = relative_cube_origin + current_cube_min + (cube_size * 0.5);
        cubes_centers.push_back(cube_center);
      }
}

void WorldDownloadManager::transformBoundingBoxAndExpand(const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,
  const Eigen::Affine3f& transform,Eigen::Vector3f& bbox_min_out,Eigen::Vector3f& bbox_max_out)
{
  const int SIZE = 2;
  Eigen::Vector3f inv[SIZE];
  inv[0] = bbox_min;
  inv[1] = bbox_max;
  Eigen::Vector3f comb;
  for (uint ix = 0; ix < SIZE; ix++)
  {
    comb.x() = inv[ix].x();
    for (uint iy = 0; iy < SIZE; iy++)
    {
      comb.y() = inv[iy].y();
      for (uint iz = 0; iz < SIZE; iz++)
      {
        comb.z() = inv[iz].z();
        const Eigen::Vector3f t_comb = transform * comb;
        if (!ix && !iy && !iz) // first iteration
          bbox_min_out = bbox_max_out = t_comb;
        else
        {
          bbox_min_out = bbox_min_out.array().min(t_comb.array());
          bbox_max_out = bbox_max_out.array().max(t_comb.array());
        }
      }
    }
  }
}

TIncompletePointsListener::PointXYZNormalCloud::Ptr TIncompletePointsListener::ClearBBoxCloud(
  const PointXYZNormalCloud::ConstPtr cloud, const Eigen::Vector3f & bbox_min, const Eigen::Vector3f & bbox_max)
  {
  PointXYZNormalCloud::Ptr result(new PointXYZNormalCloud);
  const size_t size = cloud->size();
  result->reserve(size);
  for (uint i = 0; i < size; i++)
    {
    const pcl::PointNormal & pt = (*cloud)[i];
    const Eigen::Vector3f ept(pt.x,pt.y,pt.z);
    if ((ept.array() < bbox_min.array()).any() || (ept.array() > bbox_max.array()).any())
      result->push_back(pt);
    }
  return result;
  }

Transform<float, 3, Affine, AutoAlign> Plane::get2DArbitraryRefSystem() const
{
    Eigen::Vector3f n = normal_.getUnitNormal();

    /// seek for a good unit axis to project on plane
    /// and then use it as X direction of the 2d local system
    size_t min_id;
    n.array().abs().minCoeff(&min_id);

    DLOG(INFO) << "min id is " << min_id;
    Vector3f proj_axis = Vector3f::Zero();
    proj_axis(min_id) = 1; // unity on that axis

    // project the selected axis on the plane
    Vector3f second_ax = projectVectorOnPlane(proj_axis);
    second_ax.normalize();

    Vector3f first_ax = n.cross(second_ax);

    first_ax.normalize();

    Transform<float, 3, Affine, AutoAlign> T;
//    T.matrix().fill(0);
    T.matrix().col(0).head(3) = first_ax;
    T.matrix().col(1).head(3) = second_ax;
    T.matrix().col(2).head(3) = n;
//    T.matrix()(3, 3) = 1;


    DLOG(INFO) << "Transform computed \n " << T.inverse().matrix() << "normal was " << n;

    DLOG(INFO) << "In fact T*n " <<T.inverse() *n;
    return T.inverse();
}

png::image<png::rgb_pixel_16> bumpier::model::calculate_normals(const png::image<png::gray_pixel_16>& input, double phi, space_type space) const
{
    int width = input.get_width(), height = input.get_height();
    png::image<png::rgb_pixel_16> output(width, height);

    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++)
        {
            int xmin = (x + width  - 1) % width,
                ymin = (y + height - 1) % height,
                xmax = (x + 1) % width,
                ymax = (y + 1) % height;

            Eigen::Vector3f normal = (position(input, x, ymax) - position(input, x, ymin)).cross(
                                      position(input, xmax, y) - position(input, xmin, y));

            if(space == tangent_space)
				normal = tangentspace(Eigen::Vector2f((float)x / width, (float)y / height)) * normal;

			normal.normalize();

            normal = (normal.array() * 0.5 + 0.5) * 0xFFFF;
            output.set_pixel(x, y, png::rgb_pixel_16(normal[0], normal[1], normal[2]));
        }

    return output;
}

void WorldDownloadManager::initRaycaster(bool has_intrinsics,const kinfu_msgs::KinfuCameraIntrinsics & intr,
  bool has_bounding_box_view,const kinfu_msgs::KinfuCloudPoint & bbox_min,const kinfu_msgs::KinfuCloudPoint & bbox_max)
{
  const uint rows = has_intrinsics ? intr.size_y : m_kinfu->rows();
  const uint cols = has_intrinsics ? intr.size_x : m_kinfu->cols();

  float cx,cy,fx,fy;
  float min_range = 0.0;

  if (has_intrinsics)
  {
    ROS_INFO("kinfu: custom intrinsics will be used.");
    cx = intr.center_x;
    cy = intr.center_y;
    fx = intr.focal_x;
    fy = intr.focal_y;
    min_range = intr.min_range;
  }
  else
    m_kinfu->getDepthIntrinsics(fx,fy,cx,cy);

  if (!m_raycaster || (uint(m_raycaster->rows) != rows) || (uint(m_raycaster->cols) != cols))
  {
    ROS_INFO("kinfu: initializing raycaster...");
    m_raycaster = RayCaster::Ptr(new RayCaster(rows,cols));
  }

  m_raycaster->setRaycastStep(m_kinfu->volume().getTsdfTruncDist() * 0.6);
  m_raycaster->setMinRange(min_range);
  m_raycaster->setIntrinsics(fx,fy,cx,cy);

  if (has_bounding_box_view)
    {
    const Eigen::Vector3f im(bbox_min.x,bbox_min.y,bbox_min.z);
    const Eigen::Vector3f iM(bbox_max.x,bbox_max.y,bbox_max.z);
    ROS_INFO("kinfu: raycaster will be limited to bounding box: %f %f %f - %f %f %f",im.x(),im.y(),im.z(),iM.x(),iM.y(),iM.z());
    const Eigen::Vector3f m = m_reverse_initial_transformation.inverse() * im;
    const Eigen::Vector3f M = m_reverse_initial_transformation.inverse() * iM;
    const Eigen::Vector3f mmin = m.array().min(M.array());
    const Eigen::Vector3f mmax = m.array().max(M.array());

    m_raycaster->setBoundingBox(mmin,mmax);
    }
  else
    m_raycaster->clearBoundingBox();
}

    /** \brief Constructor
     * param[in] volume_size size of the volume in mm
     * param[in] volume_res volume grid resolution (typically device::VOLUME_X x device::VOLUME_Y x device::VOLUME_Z)
     */
    DeviceVolume (const Eigen::Vector3f &volume_size, const Eigen::Vector3i &volume_res)
        : volume_size_ (volume_size)
    {
        // initialize GPU
        device_volume_.create (volume_res[1] * volume_res[2], volume_res[0]); // (device::VOLUME_Y * device::VOLUME_Z, device::VOLUME_X)
        pcl::device::initVolume (device_volume_);

        // truncation distance
        Eigen::Vector3f voxel_size = volume_size.array() / volume_res.array().cast<float>();
        trunc_dist_ = max ((float)min_trunc_dist, 2.1f * max (voxel_size[0], max (voxel_size[1], voxel_size[2])));
    };

  void onGMM(const gaussian_mixture_model::GaussianMixture & mix)
  {
    visualization_msgs::MarkerArray msg;
    ROS_INFO("gmm_rviz_converter: Received message.");

    uint i;

    for (i = 0; i < mix.gaussians.size(); i++)
    {
      visualization_msgs::Marker marker;

      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      marker.id = i;
      marker.type = visualization_msgs::Marker::SPHERE;
      marker.action = visualization_msgs::Marker::ADD;
      marker.lifetime = ros::Duration();

      Eigen::Vector3f coords;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]);
      marker.pose.position.x = coords.x();
      marker.pose.position.y = coords.y();
      marker.pose.position.z = coords.z();

      Eigen::Matrix3f covmat;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++)
          covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic);

      Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat);

      Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real();
      if (eigenvectors.determinant() < 0.0)
        eigenvectors.col(0) = - eigenvectors.col(0);
      Eigen::Matrix3f rotation = eigenvectors;
      Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation));

      marker.pose.orientation.x = quat.x();
      marker.pose.orientation.y = quat.y();
      marker.pose.orientation.z = quat.z();
      marker.pose.orientation.w = quat.w();

      Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real();
      Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt());
      if (m_normalize)
        scale.normalize();
      marker.scale.x = mix.weights[i] * scale.x() * m_scale;
      marker.scale.y = mix.weights[i] * scale.y() * m_scale;
      marker.scale.z = mix.weights[i] * scale.z() * m_scale;

      marker.color.a = 1.0;
      rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b);

      msg.markers.push_back(marker);
    }

    // this a waste of resources, but we need to delete old markers in some way
    // someone should add a "clear all" command to rviz
    // (using expiration time is not an option, here)
    for ( ; i < m_max_markers; i++)
    {
      visualization_msgs::Marker marker;
      marker.id = i;
      marker.action = visualization_msgs::Marker::DELETE;
      marker.lifetime = ros::Duration();
      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      msg.markers.push_back(marker);
    }

    m_pub.publish(msg);
    ROS_INFO("gmm_rviz_converter: Sent message.");
  }

 void SemanticLabelsVisualization::multiplyColors(Eigen::Vector3i& rgb, Eigen::Vector3f label)
 {
   rgb = (rgb.cast<float>().array() * label.array()).cast<int>().matrix();
 }