vnl_vector<double> psciob::TransformBoundingBox(const vnl_vector<double> &inputBB, const vnl_matrix<double> &transformMatrix) { unsigned int n = transformMatrix.rows(); unsigned int len = inputBB.size(); unsigned int D = n-1; if ( (n!=transformMatrix.rows()) || (len!=2*D) ) { std::cout<<"-- size inputBB= "<<inputBB.size()<<", size transformMatrix= "<<transformMatrix.rows()<<" ; matrix = "<<transformMatrix<<std::endl; throw DeformableModelException("Error in TransformBoundingBox : inconsistent inputs"); } vnl_vector<double> outputBB(len); vnl_matrix<double> rot(D,D); vnl_vector<double> trans(D); rot = transformMatrix.extract(D,D,0,0); trans = transformMatrix.extract(D,1,0,D).get_column(0); vnl_vector<double> p1(D), p2(D), p3(D), p4(D), p5(D), p6(D), p7(D), p8(D); vnl_vector<double> q1(D), q2(D), q3(D), q4(D), q5(D), q6(D), q7(D), q8(D); switch(D) { case 2: p1(0) = inputBB(0); p1(1) = inputBB(2); p2(0) = inputBB(1); p2(1) = inputBB(2); p3(0) = inputBB(1); p3(1) = inputBB(3); p4(0) = inputBB(0); p4(1) = inputBB(3); q1 = rot*p1 + trans; q2 = rot*p2 + trans; q3 = rot*p3 + trans; q4 = rot*p4 + trans; outputBB(0)= q1(0); outputBB(1)= q1(0); outputBB(2)= q1(1); outputBB(3)= q1(1); for (unsigned int i=0 ; i<D ; i++) { outputBB(2*i) = min(outputBB(2*i), q2(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q2(i)); outputBB(2*i) = min(outputBB(2*i), q3(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q3(i)); outputBB(2*i) = min(outputBB(2*i), q4(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q4(i)); } break; case 3: p1(0) = inputBB(0); p1(1) = inputBB(2); p1(2) = inputBB(4); p2(0) = inputBB(1); p2(1) = inputBB(2); p2(2) = inputBB(4); p3(0) = inputBB(1); p3(1) = inputBB(3); p3(2) = inputBB(4); p4(0) = inputBB(0); p4(1) = inputBB(3); p4(2) = inputBB(4); p5(0) = inputBB(0); p5(1) = inputBB(2); p5(2) = inputBB(5); p6(0) = inputBB(1); p6(1) = inputBB(2); p6(2) = inputBB(5); p7(0) = inputBB(1); p7(1) = inputBB(3); p7(2) = inputBB(5); p8(0) = inputBB(0); p8(1) = inputBB(3); p8(2) = inputBB(5); q1 = rot*p1 + trans; q2 = rot*p2 + trans; q3 = rot*p3 + trans; q4 = rot*p4 + trans; q5 = rot*p5 + trans; q6 = rot*p6 + trans; q7 = rot*p7 + trans; q8 = rot*p8 + trans; outputBB(0)= q1(0); outputBB(1)= q1(0); outputBB(2)= q1(1); outputBB(3)= q1(1); outputBB(4)= q1(2); outputBB(5)= q1(2); for (unsigned int i=0 ; i<D ; i++) { outputBB(2*i) = min(outputBB(2*i), q2(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q2(i)); outputBB(2*i) = min(outputBB(2*i), q3(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q3(i)); outputBB(2*i) = min(outputBB(2*i), q4(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q4(i)); outputBB(2*i) = min(outputBB(2*i), q5(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q5(i)); outputBB(2*i) = min(outputBB(2*i), q6(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q6(i)); outputBB(2*i) = min(outputBB(2*i), q7(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q7(i)); outputBB(2*i) = min(outputBB(2*i), q8(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q8(i)); } break; default : throw DeformableModelException("Error in TransformBoundingBox : expecting 2 or 3 dimensional spaces only"); break; } return outputBB; }
int select_points(const vnl_matrix<double>&pts, const std::vector<int>&index, vnl_matrix<double>& selected) { int n = index.size(); int d = pts.cols(); selected.set_size(n,d); for (int i = 0; i < n; ++i) { selected.update(pts.extract(1, d, index[i]), i); } return n; }
vnl_vector<double> Rigid2DTransform::GetParametersFromMatrix(const vnl_matrix<double> &transformMatrix) const { vnl_vector<double> params(m_nbParams); vnl_matrix<double> rotmat = transformMatrix.extract(m_nbDimensions,m_nbDimensions,0,0); params(0) = transformMatrix(0,m_nbDimensions); params(1) = transformMatrix(1,m_nbDimensions); params(2) = psciob::GetAngleFrom2DRotationMatrix(rotmat); return params; }
vnl_vector<double> Rigid3DTransform::GetParametersFromMatrix(const vnl_matrix<double> &transformMatrix) const { vnl_vector<double> params(m_nbParams); params(0) = transformMatrix(0,m_nbDimensions); params(1) = transformMatrix(1,m_nbDimensions); params(2) = transformMatrix(2,m_nbDimensions); vnl_vector<double> tmpvect = GetEulerAnglesFrom3DRotationMatrix(transformMatrix.extract(3,3,0,0)); params(3) = tmpvect(0); params(4) = tmpvect(1); params(5) = tmpvect(2); return params; }