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; }