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;
}
bool UnivariateMixturePDF::SetParameters(const vnl_vector<double> &p) {
if (p.size()!=m_totalNbParams) return false;
unsigned cumSumNbParams=0;
//set the parameters of each law, in turn...
for (unsigned i=0 ; i<m_univ_pdfs.size() ; i++) {
if (!m_univ_pdfs[i]->SetParameters(p.extract(m_nbP[i] ,cumSumNbParams))) {
//rewind modifications if those parameters are invalid
for (int j=i-1 ; j>=0 ; j--) {
cumSumNbParams -= m_nbP[j];
if (!m_univ_pdfs[j]->SetParameters(m_params.extract(m_nbP[j] ,cumSumNbParams))) throw DeformableModelException("UnivariateMixturePDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
}
return false;
}
cumSumNbParams+=m_nbP[i];
}
m_params = p;
return true;
}
// 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;
}
