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

/// \brief		Unscented transform of process Sigma points
void UnscentedKalmanFilter::utf(vnl_matrix<double> X, vnl_vector<double> u,
	vnl_vector<double> &y, vnl_matrix<double> &Y, vnl_matrix<double> &P, vnl_matrix<double> &Y1)
{
	// determine number of sigma points
	unsigned int L = X.cols();
	// zero output matrices
	y.fill(0.0); Y.fill(0.0);

	// transform the sigma points and put them as columns in a matrix Y
	for( int k = 0; k < L; k++ )
	{
		vnl_vector<double> xk = X.get_column(k);
		vnl_vector<double> yk(N);
		f(xk,u,yk);
		Y.set_column(k,yk);
		// add each transformed point to the weighted mean
		y = y + Wm.get(0,k)*yk;
	}

	// create a matrix with each column being the weighted mean
	vnl_matrix<double> Ymean(N,L);
	for( int k = 0; k < L; k++ )
		Ymean.set_column(k,y);
	// set the matrix of difference vectors
	Y1 = Y-Ymean;
	// calculate the covariance matrix output
	vnl_matrix<double> WC(L,L,0.0);
	WC.set_diagonal(Wc.get_row(0));
	P = Y1*WC*Y1.transpose();
}

/// \brief		Nonlinear process model for needle steering
void UnscentedKalmanFilter::f(vnl_vector<double> x1, vnl_vector<double> u, 
	vnl_vector<double> &x2)
{
	// initialize the output to 0
	x2.fill(0.0);
	// isolate the position and orientation components of the input vector
	vnl_vector<double> p = x1.extract(3,0);
	vnl_vector<double> r = x1.extract(3,3);
	// change rotation vector representation to quaternion
	vnl_vector<double> r_norm = r;
	r_norm.normalize();
	vnl_quaternion<double> q(r_norm,r.magnitude());
	
	// isolate specific input variables
	double d_th = u[0];
	double ro = u[1];
	double l = u[2];

	// define x,z axes as vectors
	vnl_matrix<double> I(3,3); I.set_identity();
	vnl_vector<double> x_axis = I.get_column(0);
	vnl_vector<double> z_axis = I.get_column(2);

	// Update position
	
	// define rotation matrix for d_th about z_axis
	vnl_matrix<double> Rz_dth = vnl_rotation_matrix( (d_th*z_axis) );
	
	// define circular trajectory in y-z plane
	vnl_vector<double> circ(3,0.0);
	circ[1] = ro*(1-cos(l/ro));
	circ[2] = ro*sin(l/ro);

	// define delta position vector in current frame
	vnl_vector<double> d_p = Rz_dth*circ;

	// Transform delta vector into world frame using quaternion rotation
	vnl_vector<double> d_p_world = q.rotate(d_p);

	// add rotated vector to original position
	vnl_vector<double> p2 = d_p_world + p;

	// Update orientation

	// form quaternions for x-axis and z-axis rotations
	vnl_quaternion<double> q_b(z_axis,d_th);
	vnl_quaternion<double> q_a(x_axis,-l/ro);
	// multiply original orientation quaternion
	vnl_quaternion<double> q2 = q*q_b*q_a;
	vnl_vector<double> r2 = q2.axis()*q2.angle();

	// Compose final output
	for( int i = 0; i < 3; i++)
	{
		x2[i] = p2[i];
		x2[i+3] = r2[i];
	}
}

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 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++)
  {
//.........这里部分代码省略.........