C++ vnl_vector::set_size方法代码示例

// Initialize mu and eta by calling the corresponding
// link and distribution functions on each row
static void _InitMuEta(mitk::DistSimpleBinominal *dist, mitk::LogItLinking *link, const vnl_vector<double> &yData, vnl_vector<double> &mu, vnl_vector<double> &eta)
{
  int rows = yData.size();
  mu.set_size(rows);
  eta.set_size(rows);
  for (int r = 0; r < rows; ++r)
  {
    mu(r) = dist->Init(yData(r));
    eta(r) = link->Link(mu(r));
  }
}

void getDynamicBackgroundSumLogProb(IplImage *smooth,
                                    const vnl_vector<FLOAT> &bgProb,
                                    vnl_vector<FLOAT> &sumPix,
                                    FLOAT &sum)
{
  unsigned int widthExtra=smooth->width+1; // each line needs an extra leading zero
  unsigned int imgSizeExtra=widthExtra*smooth->height;

  if (sumPix.size()!=imgSizeExtra)
    sumPix.set_size(imgSizeExtra);

  int pixelInd=0,channelInd=0;
  sum=0;
  for (unsigned int i=0;i<imgSizeExtra;i++)
  {
    if (i%widthExtra==0) // add leading zero
      sumPix(i)=0;
    else
    {
      sumPix(i)=sumPix(i-1)+bgProb(pixelInd)+3.0*log(256); // - (-log ...) , something to do with foreground probablity
      sum+=bgProb(pixelInd);
      pixelInd+=1;channelInd+=3; // next pixel
    }
  }
}

bool arlCore::fieldCalibration( PointList::csptr real, PointList::csptr distorded, unsigned int degree, vnl_vector<double> &parameters, double &RMS )
{
    if(degree<1) return false;
    Polynomial_cost_function pcf( real, distorded, degree );
    vnl_powell op(&pcf);
    parameters.set_size(pcf.getNbParameters());
    parameters.fill(0.0);
    op.minimize(parameters);
    RMS = op.get_end_error();
    return true;
}

void TpsRegistration::SetParam(vnl_vector<double>& x0) {
  int k = 0;
  x0.set_size(func_->get_number_of_unknowns());
  x0.fill(0);
  if (!func_->fix_affine_) { // x0 includes affine
    for (int i = 0; i < param_affine_.rows(); ++i) {
      for (int j = 0; j < d_; ++j, ++k) {
        x0[k] = param_affine_(i, j);
      }
    }
  }
  for (int i = 0; i < param_tps_.rows(); ++i) {
    for (int j = 0; j < d_; ++j, ++k) {
      x0[k] = param_tps_(i, j);
    }
  }
}

void normalize(vnl_matrix<double>& x,
    vnl_vector<double>& centroid, double& scale) {
  int n = x.rows();
  if (n==0) return;
  int d = x.cols();
  centroid.set_size(d);

  vnl_vector<double> col;
  for (int i = 0; i < d; ++i) {
    col = x.get_column(i);
    centroid(i) = col.mean();
  }
  for (int i = 0; i < n; ++i) {
    x.set_row(i, x.get_row(i) - centroid);
  }
  scale = x.frobenius_norm() / sqrt(double(n));
  x = x / scale;
}

void getDynamicBackgroundLogProb(IplImage *smooth,
                                 GaussianMixture<DYNBG_GAUS,DYNBG_TYPE,DYNBG_MAXGAUS> *gaussianMixtures,
                                 vnl_vector<FLOAT> &bgProb)
{
  unsigned int size=smooth->width*smooth->height;
  if (bgProb.size()!=size)
    bgProb.set_size(size);
  bgProbMin=std::numeric_limits<float>::max();
  bgProbMax=-std::numeric_limits<float>::max();

  // threaded version
  boost::thread threads[nrThreads];
  for (unsigned i=0;i<nrThreads;i++)
  {
    unsigned begin=i*(size/nrThreads);
    unsigned end=(i+1)*(size/nrThreads);
    if (i==nrThreads-1) // is last
      end=size;
    threads[i]=boost::thread(DynBackGroundThread(begin,end,
                                                 smooth,gaussianMixtures,bgProb));
  }
  for (unsigned i=0;i<nrThreads;i++)
    threads[i].join();

  /* // non-threaded version
  int updateGaussianID;
  DYNBG_TYPE data[DYNBG_DIM],squareDist[DYNBG_DIM];
  int channelInd=0;
  for (unsigned int i=0;i<size;i++)
  {
    data[0]=(unsigned char)(smooth->imageData[channelInd+0]);
    data[1]=(unsigned char)(smooth->imageData[channelInd+1]);
    data[2]=(unsigned char)(smooth->imageData[channelInd+2]);
    DYNBG_TYPE logProbBG=gaussianMixtures[i].logProbability(data,squareDist,minWeight,squareMahanobisMatch,updateGaussianID);
    gaussianMixtures[i].update(data,initVar,decay,weightReduction,updateGaussianID);
    bgProb(i)=logProbBG;
    if (logProbBG<bgProbMin) bgProbMin=logProbBG;
    if (logProbBG>bgProbMax) bgProbMax=logProbBG;
    channelInd+=3; // next pixel
  }
  */
}

void blIALinearSampler3D::SampleLinearRaw( vnl_vector<double>& profile )
{
	vtkProbeFilter* probe = vtkProbeFilter::New();

	probe->SetSource( m_Image );
	probe->SetInput(m_SamplerPositions);

	
	probe->Update();

	vtkDataSet* samples = probe->GetOutput();

	vtkIdType nsamples = samples->GetPointData()->GetScalars()->GetNumberOfTuples();
	
	profile.set_size(nsamples);

	for(int i=0; i<nsamples; i++ )
	{
		double *value = samples->GetPointData()->GetScalars()->GetTuple(i);
		profile[i] = value[0];
	}

	probe->Delete();
}

void histogram(const vector<CvPoint> &tplt, IplImage *img, vnl_vector<double> &hist, unsigned iPerBin = 1)
{
  vector<scanline_t> mask;
  getMask(tplt,mask);
  if (hist.size() == 0) {
    hist.set_size(256/iPerBin);
    hist.fill(0.0);
  }
  unsigned char *src = (unsigned char *)img->imageData;
  unsigned K = img->nChannels, denom = K*iPerBin;

  for (unsigned i=0; i<mask.size(); ++i) {
    unsigned char *s=src+mask[i].line*img->widthStep + K*mask[i].start;
    for (unsigned j=mask[i].start; j<mask[i].end; ++j) {
      unsigned val=0.;
      for (unsigned k=0; k!=K; ++k, ++s) {
        val += *s;
        *s = 0;
      }
      val /= denom;
      hist[val]++;
    }
  }
}

int main(int argc, char **argv)
{
  string params_file;

  // handling arguments
  po::options_description optionsDescription(
      "Human Detection main function\n"
      "Available remappings:\n"
      "  humanDetections:=<humanDetections-topic>\n"
      "\n"
      "Allowed options");
  optionsDescription.add_options()
    ("help,h","show help message")
    ("params_file,p", po::value<string>(&params_file)->required(),"filename of params.xml")
    ("num_persons,n", po::value<unsigned int>(&NUM_PERSONS)->default_value(4))
    ("visualize,v","visualize detection\n")
    ("threads,t", po::value<unsigned int>(&nrThreads)->default_value(4),"number of threads used");
    ("save_all,a", po::value<string>(&save_all)->default_value(""),"save all data\n");

  po::variables_map variablesMap;

  try
  {
    po::store(po::parse_command_line(argc, argv, optionsDescription),variablesMap);
    if (variablesMap.count("help")) {cout<<optionsDescription<<endl; return 0;}
    po::notify(variablesMap);
  }
  catch (const std::exception& e)
  {
    std::cout << "--------------------" << std::endl;
    std::cerr << "- " << e.what() << std::endl;
    std::cout << "--------------------" << std::endl;
    std::cout << optionsDescription << std::endl;
    return 1;
  }

  visualize=variablesMap.count("visualize"); // are we visualizing the frames and detections?

  if (!save_all.empty())
  {
    sprintf(data_file,"%s/data.xml",save_all.c_str());
  }

  string path, bgmodel_file, prior_file, frame_file;
  boost::filesystem::path p(params_file);
  path = p.parent_path().string().c_str();
  bgmodel_file = path + "/" + "bgmodel.xml";
  prior_file = path + "/" + "prior.txt";
  frame_file = path + "/" + "frame.dat";

  
  ROS_INFO_STREAM("loading '"<<bgmodel_file.c_str()<<"'");
  // load coordinate frame of camera
  if (!load_msg(frame,frame_file))
  {
    std::cerr<<"Failed to load the coordinate frame from file '"<<frame_file<<"'. Use program 'create_frame' to create the file."<<endl;
    exit(1);
  }

  ROS_INFO_STREAM("loading '"<<bgmodel_file.c_str()<<"'");
  // Initialize localization module
#if USE_DYNAMIC_BACKGROUND
#else
  getBackground(bgmodel_file.c_str(), bgModel); // load background model
#endif
  ROS_INFO_STREAM("loading '"<<params_file.c_str()<<"'");
  loadCalibrations(params_file.c_str()); // load calibration
  ROS_INFO_STREAM("loading '"<<prior_file.c_str()<<"'");
  loadHull(prior_file.c_str(), priorHull);

#if USE_DYNAMIC_BACKGROUND
#else
  assert_eq(bgModel.size(), CAM_NUM);
  assert_eq(cam.size(), bgModel.size());
#endif
  ROS_INFO_STREAM("generate Scan locations"); 
  genScanLocations(priorHull, scanres, scanLocations);

  logLocPrior.set_size(scanLocations.size());
  logLocPrior.fill(-log(scanLocations.size()));
  ROS_INFO_STREAM("loading done\n"); 

  // ROS nodes, subscribers and publishers
  ros::init(argc, argv, "camera_localization");
  ros::NodeHandle n;
  std::string resolved_humanDetections=n.resolveName("humanDetections");
  cout<<"publish to detection topic: "<<resolved_humanDetections<<endl;

  tf::TransformBroadcaster transformBroadcaster;
  transformBroadcasterPtr=&transformBroadcaster;
  
  image_transport::ImageTransport it(n);
  humanDetectionsPub = n.advertise<accompany_uva_msg::HumanDetections>(resolved_humanDetections, 10);
  markerArrayPub = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",0);

  frameCounter=new FrameCounter(cam.size());
  backgroundsPub=new image_transport::Publisher[cam.size()];
  detectionsPub=new image_transport::Publisher[cam.size()];
  for (unsigned int i=0;i<cam.size();i++)
  {
//.........这里部分代码省略.........