cv::Mat sg::padKernelFFT( cv::Mat const & input, cv::Size const & paddedSize )
{
cv::Mat result = cv::Mat::zeros( paddedSize, input.type() );
// Get the 4 quadrants of the input kernel
int hwx = input.cols / 2;
int hwy = input.rows / 2;
int px = hwx;
int py = hwy;
int fftW = paddedSize.width;
int fftH = paddedSize.height;
if( input.cols %2 != 0 )
++px;
if( input.rows % 2 != 0 )
++py;
cv::Mat q0( input, cv::Rect( 0, 0, px, py ) );
cv::Mat q02( result, cv::Rect( fftW - px, fftH - py, px, py ) );
cv::Mat q1( input, cv::Rect( px, 0, hwx, py ) );
cv::Mat q12( result, cv::Rect( 0, fftH - py, hwx, py ) );
cv::Mat q2( input, cv::Rect( 0, py, px, hwy ) );
cv::Mat q22( result, cv::Rect( fftW - px, 0, px, hwy ) );
cv::Mat q3( input, cv::Rect( px, py, hwx, hwy ) );
cv::Mat q32( result, cv::Rect( 0, 0, hwx, hwy ) );
q0.copyTo( q02 );
q1.copyTo( q12 );
q2.copyTo( q22 );
q3.copyTo( q32 );
// if our filter doesn't have real and complex components, make the complex part zeros
if( result.channels() < 2 )
{
cv::Mat planes[] = { result, cv::Mat::zeros( result.size(), CV_32F ) };
cv::merge( planes, 2, result );
}
return result;
}
void sg::fftshift( cv::Mat & src )
{
// If even we can just pivot around the center pixel
// if odd, we pivot around center + (1,1)
// quadrants arranged q0 q1
// q2 q3
// q0 is located at (0, 0) with width px, height py
// q1 is located at (px, 0) with width hw, height py
// q2 is located at (0, py) with width px, height hw
// q3 is located at (px, py) with width hw, height hw
int hwx = src.cols / 2;
int hwy = src.rows / 2;
int px = hwx;
int py = hwy;
if( src.cols %2 != 0 )
++px;
if( src.rows % 2 != 0 )
++py;
cv::Mat q0( src, cv::Rect( 0, 0, px, py ) );
cv::Mat q02( src, cv::Rect( hwx, hwy, px, py ) );
cv::Mat q1( src, cv::Rect( px, 0, hwx, py ) );
cv::Mat q12( src, cv::Rect( 0, hwy, hwx, py ) );
cv::Mat q2( src, cv::Rect( 0, py, px, hwy ) );
cv::Mat q22( src, cv::Rect( hwx, 0, px, hwy ) );
cv::Mat q3( src, cv::Rect( px, py, hwx, hwy ) );
cv::Mat q32( src, cv::Rect( 0, 0, hwx, hwy ) );
// q0 is the biggest segment and might overlap with
// everything else so it gets done last, we hold it in tmp
cv::Mat tmpq0, tmpq2;
q0.copyTo( tmpq0 );
q2.copyTo( tmpq2 );
q3.copyTo( q32 );
q1.copyTo( q12 );
tmpq2.copyTo( q22 );
tmpq0.copyTo( q02 );
}
// constructor from data
void NewtonEulerDSTest::testNewtonEulerDSQuaternion()
{
std::cout << "--> Test: quaternion 1 from position" <<std::endl;
SP::SiconosVector axis(new SiconosVector(3));
SiconosVector axisref(3);
double angle= 1e24;
double angleref = 1e24;
angle = ::getAxisAngle(q0, axis );
std::cout << "q0 angle : " <<angle<<std::endl;
std::cout << "q0 axis : " << std::endl;
axis->display();
axisref(0)= 1.0;
axisref(1)= 0.0;
axisref(2)= 0.0;
angleref = M_PI;
SP::SimpleMatrix R(new SimpleMatrix(3,3));
SimpleMatrix Rref(3,3);
::computeRotationMatrix(q0, R);
R->display();
Rref.eye();
Rref(1,1) =-1.0;
Rref(2,2) = -1.0;
SP::SiconosVector v(new SiconosVector(3));
SP::SiconosVector vref(new SiconosVector(3));
(*v)(0)=1.0;
(*v)(1)=1.0;
(*v)(2)=1.0;
::rotateAbsToBody(q0, v );
std::cout << "v : "<<std::endl;
v->display();
(*vref)(0)=1.0;
(*vref)(1)=-1.0;
(*vref)(2)=-1.0;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionA : ", angle == angleref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionB : ", *(axis) == axisref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionC : ", *(R) == Rref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionC : ", *(v) == *(vref), true);
angle =::getAxisAngle(q01, axis );
std::cout << "q01 angle : " <<angle<<std::endl;
std::cout << "q01 axis : " << std::endl;
axis->display();
axisref(0)= 0.0;
axisref(1)= 0.0;
axisref(2)= 0.0;
angleref = 0.0;
Rref.eye();
::computeRotationMatrix(q01, R);
R->display();
Rref.display();
::rotateAbsToBody(q01, v );
std::cout << "v : "<<std::endl;
v->display();
(*vref)(0)=1.0;
(*vref)(1)=-1.0;
(*vref)(2)=-1.0;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", angle == angleref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", *(axis) == axisref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", *(R) == Rref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", *(v) == *(vref), true);
std::cout <<" ---------- test with q02 (rotation of pi/2 about the y-axis)" << std::endl;
SP::SiconosVector q02(new SiconosVector(7));
q02->zero();
angle=M_PI_2;
axis->zero();
(*axis)(1)= 1.0;
std::cout << "q02 angle : " <<angle<<std::endl;
std::cout << "q02 axis : " << std::endl;
axis->display();
::setAxisAngle(q02,axis,angle);
std::cout << "q02 : " << std::endl;
q02->display();
SP::SiconosVector q02ref(new SiconosVector(7));
q02ref->zero();
(*q02ref)(3) = cos(angle/2.0);
(*q02ref)(5) = sin(angle/2.0);
q02ref->display();
::computeRotationMatrix(q02, R);
R->display();
Rref.zero();
Rref(2,0)=-1.0;
Rref(1,1)=1.0;
Rref(0,2)=1.0;
Rref.display();
::rotateAbsToBody(q02, v );
std::cout << "v : "<<std::endl;
v->display();
(*vref)(0)=-1.0;
(*vref)(1)=-1.0;
(*vref)(2)=-1.0;
std::cout << "vref : "<<std::endl;
vref->display();
std::cout << (*v-*vref).normInf()<<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionG : ", *(q02) == *(q02ref) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionH : ", *(R) == Rref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", *(v) == *(vref), true);
std::cout <<" ---------- test with q03 (rotation of pi/4 about the y-axis)" << std::endl;
SP::SiconosVector q03(new SiconosVector(7));
q03->zero();
angle=M_PI_4;
axis->zero();
(*axis)(1)= 1.0;
std::cout << "q03 angle : " <<angle<<std::endl;
std::cout << "q03 axis : " << std::endl;
axis->display();
::setAxisAngle(q03,axis,angle);
std::cout << "q03 : " << std::endl;
q03->display();
SP::SiconosVector q03ref(new SiconosVector(7));
q03ref->zero();
(*q03ref)(3) = cos(angle/2.0);
(*q03ref)(5) = sin(angle/2.0);
q03ref->display();
::computeRotationMatrix(q03, R);
R->display();
Rref.zero();
Rref(0,0)=sqrt(2.0)/2.0;
Rref(0,2)=sqrt(2.0)/2.0;
Rref(1,1)=1.0;
Rref(2,0)=-sqrt(2.0)/2.0;
Rref(2,2)=sqrt(2.0)/2.0;
Rref.display();
::rotateAbsToBody(q03, v);
std::cout << "v : "<<std::endl;
v->display();
(*vref)(0)=-sqrt(2.0);
(*vref)(1)=-1.0;
(*vref)(2)=0.0;
std::cout << "vref : "<<std::endl;
vref->display();
std::cout << (*v-*vref).normInf()<<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionG : ", *(q03) == *(q03ref) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternionH : ", *(R) == Rref, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testNewtonEulerDSQuaternion : ", ((*v-*vref).normInf() <= std::numeric_limits<double>::epsilon()*10.0), true);
std::cout << "--> quaternion 2 test ended with success." <<std::endl;
}
