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

bool LinearCombinationPDF::SetParameters(const vnl_vector<double> &p) {
	if (p.size()!=m_nbP_a+m_nbP_b+m_nbP_x) return false;
	//vnl_vector<double> backupParams = m_params;
	if (!m_x->SetParameters(p.extract(m_nbP_x,0))) return false;
	if (!m_a->SetParameters(p.extract(m_nbP_a, m_nbP_x))) { //undo the modifications of the previous pdfs
		if (!m_x->SetParameters(m_params.extract(m_nbP_x,0))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
		return false;
	}
	if (!m_b->SetParameters(p.extract(m_nbP_b, m_nbP_x+m_nbP_a))) { //undo the modifications of the previous pdfs
		if (!m_a->SetParameters(m_params.extract(m_nbP_a, m_nbP_x))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
		if (!m_x->SetParameters(m_params.extract(m_nbP_x,0))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
		return false;
	}
	m_params = p;
	return true;
}

/// \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];
	}
}

vnl_matrix<double> Rigid3DTransform::GetMatrixFromParameters(const vnl_vector<double> &poseParameters) const {
	vnl_matrix<double> output(4,4); 
	output.set_identity();

	vnl_matrix<double> rotmat = Get3DRotationMatrixFromEulerAngles(poseParameters.extract(3,3));
	for (unsigned i = 0 ; i<3 ; i++) {
		output(i,3) = poseParameters(i);
		for (unsigned j = 0 ; j<3 ; j++) {
			output(i,j) = rotmat(i,j);
		}
	}
	return output;
}

// loads an hrf vector from a file
bool RtDesignMatrix::loadHrfFile(vnl_vector<double> &hrf, string filename) {
  vcl_ifstream in(filename.c_str(), ios::in);
  if (in.fail()) {
    return false;
  }

  if (!hrf.read_ascii(in)) {
    return false;
  }

  // compute other parms
  hrfSamplePeriod = tr;
  hiresHrf = hrf;

  // compute the temporal derivative basis
  temporalDerivative.set_size(hrf.size());
  temporalDerivative.fill(0);
  temporalDerivative.update(hrf.extract(hrf.size() - 1, 1), 1);
  temporalDerivative = hrf - temporalDerivative;
  hiresTemporalDerivative = temporalDerivative;

  return true;
}