C++ Vector4f::head方法代码示例

void
OrganizedMultiPlaneExtractor<PointT>::compute()
{
    CHECK ( cloud_ && cloud_->isOrganized() ) << "Input cloud is not organized!";
    CHECK ( normal_cloud_ && (normal_cloud_->points.size() == cloud_->points.size() ));

    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers ( param_.min_num_plane_inliers_ );
    mps.setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
    mps.setDistanceThreshold ( param_.distance_threshold_);
    mps.setInputNormals ( normal_cloud_ );
    mps.setInputCloud ( cloud_ );

    std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
    typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
                new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
    ref_comp->setDistanceThreshold (param_.distance_threshold_, false );
    ref_comp->setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
    mps.setRefinementComparator ( ref_comp );
    mps.segmentAndRefine ( regions );

    all_planes_.clear();
    all_planes_.reserve (regions.size ());
    for (const pcl::PlanarRegion<PointT> &reg : regions )
    {
        Eigen::Vector4f plane = reg.getCoefficients();

        // flip plane normal towards viewpoint
        Eigen::Vector3f z = Eigen::Vector3f::UnitZ();
        if( z.dot(plane.head(3)) > 0 )
            plane = -plane;

      all_planes_.push_back( plane );
    }
}

bool PhotoCamera::initializeFromMeshLab(QDomElement &element)
{
    QStringList attTemp;
    //Get Translation
    attTemp = element.attribute("TranslationVector").split(" ");
    Eigen::Vector4f translation;
    for(int i = 0; i < attTemp.count(); i++){
        translation(i) = attTemp[i].toFloat();
    }
    translationMatrix.translation() = translation.head(3);
    //translationMatrix.col(3) = translation;

    //Get Center;
    attTemp = element.attribute("CenterPx").split(" ");
    principalPoint << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();


    //Get RotationMatrix;
    attTemp = element.attribute("RotationMatrix").split(" ");
    for(int i = 0; i < 4; i++){
        for(int j = 0; j < 4; j++){
            rotationMatrix(i, j) = attTemp[i*4 + j].toFloat();
        }
    }

    //Get Viewport;
    attTemp = element.attribute("ViewportPx").split(" ");
    viewport << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();

    //Lens Distortion;
    attTemp = element.attribute("LensDistortion").split(" ");
    distortion << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
    //std::cout << distortion.transpose() << std::endl;

    //PixelSize;
    attTemp = element.attribute("PixelSizeMm").split(" ");
    pixelSize << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
    oneOverDx           = 1/pixelSize(0);
    oneOverDy           = 1/pixelSize(1);
    //std::cout << pixelSize.transpose() << std::endl;

    //Focal
    focalLength = element.attribute("FocalMm").toFloat();

    buildExtrinsic();
    buildIntrinsic();
    buildProjection();

//    cameraCenter = center(intrinsicMatrix.topLeftCorner(3,4)*extrinsicMatrix.matrix());
    cameraCenter = center(intrinsicMatrix.matrix().topLeftCorner(3,4)*extrinsicMatrix.matrix());
    return true;
}

Eigen::Vector3f unproject(const Eigen::Vector3f &win,
                          const Eigen::Matrix4f &model,
                          const Eigen::Matrix4f &proj,
                          const Vector2i &viewportSize) {
    Eigen::Matrix4f Inverse = (proj * model).inverse();

    Eigen::Vector4f tmp;
    tmp << win, 1;
    tmp(0) = tmp(0) / viewportSize.x();
    tmp(1) = tmp(1) / viewportSize.y();
    tmp = tmp.array() * 2.0f - 1.0f;

    Eigen::Vector4f obj = Inverse * tmp;
    obj /= obj(3);

    return obj.head(3);
}

Eigen::Vector3f project(const Eigen::Vector3f &obj,
                        const Eigen::Matrix4f &model,
                        const Eigen::Matrix4f &proj,
                        const Vector2i &viewportSize) {
    Eigen::Vector4f tmp;
    tmp << obj, 1;

    tmp = model * tmp;

    tmp = proj * tmp;

    tmp = tmp.array() / tmp(3);
    tmp = tmp.array() * 0.5f + 0.5f;
    tmp(0) = tmp(0) * viewportSize.x();
    tmp(1) = tmp(1) * viewportSize.y();

    return tmp.head(3);
}

Eigen::Vector3f igl::unproject(
  const Eigen::Vector3f& win,
  const Eigen::Matrix4f& model,
  const Eigen::Matrix4f& proj,
  const Eigen::Vector4f& viewport)
{
  Eigen::Matrix4f Inverse = (proj * model).inverse();

  Eigen::Vector4f tmp;
  tmp << win, 1;
  tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
  tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
  tmp = tmp.array() * 2.0f - 1.0f;

  Eigen::Vector4f obj = Inverse * tmp;
  obj /= obj(3);

  return obj.head(3);
}

//TODO: move to semantics
void
PlaneExtraction::findClosestTable(std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
                                  std::vector<pcl::ModelCoefficients>& v_coefficients_plane,
                                  Eigen::Vector3f& robot_pose,
                                  unsigned int& idx)
{
  std::vector<unsigned int> table_candidates;
  for(unsigned int i=0; i<v_cloud_hull.size(); i++)
  {
    //if(fabs(v_coefficients_plane[i].values[3])>0.5 && fabs(v_coefficients_plane[i].values[3])<1.2)
    table_candidates.push_back(i);
  }
  if(table_candidates.size()>0)
  {
    for(unsigned int i=0; i<table_candidates.size(); i++)
    {
      double d_min = 1000;
      double d = d_min;
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(v_cloud_hull[i], centroid);
      //for(unsigned int j=0; j<v_cloud_hull[i].size(); j++)
      //{
      //  Eigen::Vector3f p = v_cloud_hull[i].points[j].getVector3fMap();
      //  d += fabs((p-robot_pose).norm());
      //}
      //d /= v_cloud_hull[i].size();
      Eigen::Vector3f centroid3 = centroid.head(3);
      d = fabs((centroid3-robot_pose).norm());
      ROS_INFO("d: %f", d);
      if(d<d_min)
      {
        d_min = d;
        idx = table_candidates[i];
      }
    }
  }
}

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if(argc<3){
        std::cout<<"This node is intend to use offline"<<std::endl<<"provide a logfile (produced with the dumpernode) and the output calibration file you want"<<std::endl;
        std::cout<<"example:"<<std::endl<<"offlineCalibration <input log filename> <output calibration filename>"<<std::endl;
        return 0;
    }
    float c0,c1,c2,c3=0;
    c0=1.0f;
    if(argc==7){
        c3=atof(argv[3]);
        c2=atof(argv[4]);
        c1=atof(argv[5]);
        c0=atof(argv[6]);
        std::cout<<"polynomial coeffs: d^3*"<<c3<<" + d^2*"<<c2<<" + d*"<<c1<<" + "<<c0<<std::endl;
    }
    std::fstream log;
    log.open(argv[1]);
    string topic;
    int height;
    int width;
    log >> topic;
    log >> height;
    log >> width;
    Matrix3f k;
    log >> k(0,0);
    log >> k(0,1);
    log >> k(0,2);
    log >> k(1,0);
    log >> k(1,1);
    log >> k(1,2);
    log >> k(2,0);
    log >> k(2,1);
    log >> k(2,2);
    string filename;
    //calibrationMatrix c(480,640,8000,4,8);
    int rows =480;
    int cols = 640;
    calibrationMatrix c(rows,cols,8000,4,16);
    //calibrationMatrix c(rows,cols,8000,4,64);
    while(!log.eof()){
        log>>filename;
        std::cout<< "opening "<<filename<<"\r"<<std::flush;
        cv::Mat data = cv::imread(filename,cv::IMREAD_ANYDEPTH);
        pointCloud p(data,k);
        //VOXELIZER
        pcl::PointCloud<pcl::PointXYZ>* pcl;
        pcl=p.pclCloud();
        calibration::voxelize(pcl,pcl,20);
        pointCloud p2(pcl);
        //CENTER SQUARE CLOUD
        pcl::PointCloud<pcl::PointXYZ>* square= new pcl::PointCloud<pcl::PointXYZ>();
        calibration::computeCenterSquareCloud(data,k,square,cols/10,rows/10,c0,c1,c2,c3); //100 100
        pointCloud p3(square);
        //CENTER PLANE
        Eigen::Vector4f centerModel;
        calibration::computerCenterPlane(square,centerModel,40);
        std::cout.flush();
        planeCloud centerPlaneCloud;
        centerPlaneCloud.model=centerModel;
        Eigen::Vector4f center;
        pcl::compute3DCentroid(*square,center);
        //std::cout<<"CENTROID DISTANCE: "<<center[2]<<" ( "<<center.transpose()<<" )"<<std::endl;
        centerPlaneCloud.com=center.head(3);
        //NORMALS
        pcl::PointCloud<pcl::Normal>* normals= new pcl::PointCloud<pcl::Normal>();
        calibration::computeNormals(p2.pclCloud(),normals,100);
        //REJECTION
        Eigen::Vector3f ref;
        std::vector<bool> valid;
        ref<<centerModel[0],centerModel[1],centerModel[2];
        pcl::PointCloud<pcl::PointXYZ> outFromNormalRejection;
        pcl::PointCloud<pcl::PointXYZ>* tmpCloud =p2.pclCloud();
        calibration::pointrejection(&ref,0.7f,tmpCloud,normals,&outFromNormalRejection,&valid);

        //pointCloud outFromNormalRejectionCloud(&outFromNormalRejection);
        //ERROR PER POINT
        pcl::PointCloud<pcl::PointXYZ> error;
        calibration::computeErrorPerPoint(tmpCloud,&error,center.head(3),ref,&valid);
        //std::cout<<"error cloud has "<<error.size()<<" points"<<std::endl;
        //std::cout<<"voxel cloud has "<<p2.pclCloud()->size()<<" points"<<std::endl;
        pcl::PointCloud<pcl::PointXYZ>* cloudToCalibrate=p2.pclCloud();
        //COMPUTE CALIBRATION MATRIX
        std::cout.flush();
        calibration::computeCalibrationMatrix(*cloudToCalibrate,error,k,&valid,c);
        std::cout.flush();
        //CALIBRATE POINT CLOUD
        pcl::PointCloud<pcl::PointXYZ> fixedCloud;
        calibration::calibratePointCloudWithMultipliers(*cloudToCalibrate,fixedCloud,c,k);
        std::cout.flush();

        error.clear();
        valid.clear();
        error.clear();
        cloudToCalibrate->clear();
        delete cloudToCalibrate;
        tmpCloud->clear();
        delete tmpCloud;
        p2.cloud.clear();
//.........这里部分代码省略.........