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