//____________________________________________________
//
Int_t THaBPM::Process( )
{
// called by the beam apparaturs
// uses the pedestal substracted signals from the antennas
// to get the position in the bpm coordinate system
// and uses the transformation matrix defined in the database
// to transform it into the HCS
// directions are not calculated, they are always set parallel to z
Double_t ap, am;
for (Int_t k=0; k<2; k++) {
if (fCorSignal(2*k)+fCorSignal(2*k+1)!=0.0) {
ap=fCorSignal(2*k);
am=fCorSignal(2*k+1);
fRotPos(k)=fCalibRot*(ap-am)/(ap+am);
}
else {
fRotPos(k)=0.0;
}
}
TVector dum(fRotPos);
dum*=fRot2HCSPos;
fPosition.SetXYZ(
dum(0)+fOffset(0),
dum(1)+fOffset(1),
fOffset(2)
);
return 0 ;
}
void PointProcess_addPoint (PointProcess me, double t) {
try {
if (t == NUMundefined)
Melder_throw (U"Cannot add a point at an undefined time.");
if (my nt >= my maxnt) {
/*
* Create without change.
*/
autoNUMvector <double> dum (1, 2 * my maxnt);
NUMvector_copyElements (my t, dum.peek(), 1, my nt);
/*
* Change without error.
*/
NUMvector_free (my t, 1);
my t = dum.transfer();
my maxnt *= 2;
}
if (my nt == 0 || t >= my t [my nt]) { // special case that often occurs in practice
my t [++ my nt] = t;
} else {
long left = PointProcess_getLowIndex (me, t);
if (left == 0 || my t [left] != t) {
for (long i = my nt; i > left; i --) my t [i + 1] = my t [i];
my nt ++;
my t [left + 1] = t;
}
}
} catch (MelderError) {
Melder_throw (me, U": point not added.");
}
}
TEST_F(ContextMemberTest, ComplexResetCase) {
std::weak_ptr<CoreContext> ctxtWeak;
std::weak_ptr<RefersToTweedleDee> deeWeak;
std::weak_ptr<RefersToTweedleDum> dumWeak;
{
// Set up the cycle:
AutoCreateContext ctxt;
ctxtWeak = ctxt;
ctxt->Inject<RefersToTweedleDee>();
ctxt->Inject<RefersToTweedleDum>();
// Now try to reset the cycle:
Autowired<RefersToTweedleDee> dee(ctxt);
Autowired<RefersToTweedleDum> dum(ctxt);
dee->td.reset();
// Get weak pointers so we can verify that everything cleaned up
deeWeak = dee;
dumWeak = dum;
}
// Context should have gone away by now
ASSERT_TRUE(ctxtWeak.expired()) << "Context was leaked even after a local cycle was reset";
ASSERT_TRUE(dumWeak.expired()) << "Leak detected of a member that was explicitly released";
ASSERT_TRUE(deeWeak.expired()) << "Leak detected of a member that was not explicitly released";
}
void GradGrad<D>::T_CalcElementMatrix (const FiniteElement & base_fel,
const ElementTransformation & eltrans,
FlatMatrix<SCAL> elmat,
LocalHeap & lh) const {
const CompoundFiniteElement & cfel // product space
= dynamic_cast<const CompoundFiniteElement&> (base_fel);
const ScalarFiniteElement<D> & fel_u = // u space
dynamic_cast<const ScalarFiniteElement<D>&> (cfel[GetInd1()]);
const ScalarFiniteElement<D> & fel_e = // e space
dynamic_cast<const ScalarFiniteElement<D>&> (cfel[GetInd2()]);
elmat = SCAL(0.0);
// u dofs [ru.First() : ru.Next()-1], e dofs [re.First() : re.Next()-1]
IntRange ru = cfel.GetRange(GetInd1());
IntRange re = cfel.GetRange(GetInd2());
int ndofe = re.Size();
int ndofu = ru.Size();
FlatMatrixFixWidth<D> dum(ndofu,lh); // to store grad(u-basis)
FlatMatrixFixWidth<D> dem(ndofe,lh); // to store grad(e-basis)
ELEMENT_TYPE eltype // get the type of element:
= fel_u.ElementType(); // ET_TRIG in 2d, ET_TET in 3d.
const IntegrationRule & // Note: p = fel_u.Order()-1
ir = SelectIntegrationRule(eltype, fel_u.Order()+fel_e.Order()-2);
FlatMatrix<SCAL> submat(ndofe,ndofu,lh);
submat = 0.0;
for(int k=0; k<ir.GetNIP(); k++) {
MappedIntegrationPoint<D,D> mip (ir[k],eltrans);
// set grad(u-basis) and grad(e-basis) at mapped pts in dum and dem.
fel_u.CalcMappedDShape( mip, dum );
fel_e.CalcMappedDShape( mip, dem );
// evaluate coefficient
SCAL fac = coeff_a -> T_Evaluate<SCAL>(mip);
fac *= mip.GetWeight() ;
// [ndofe x D] * [D x ndofu]
submat += fac * dem * Trans(dum) ;
}
elmat.Rows(re).Cols(ru) += submat;
if (GetInd1() != GetInd2())
elmat.Rows(ru).Cols(re) += Conj(Trans(submat));
}
bool aviIndexOdml::writeOdmlChunk()
{
uint64_t off=LMovie->Tell();
LMovie->Seek(odmlChunkPosition);
AviListAvi dum("LIST",LMovie->getFile());
dum.Begin();
dum.Write32("odml");
dum.Write32("dmlh");
dum.Write32(4);
dum.Write32(nbVideoFrame); // Real number of video frames
dum.EndAndPaddTilleSizeMatches(248+12);
LMovie->Seek(off);
return true;
}
bool RegexSourceSink::GrabFrame(ImagePacket &target, int indexIncrement)
{
if (index+indexIncrement<goodFiles->count() && index+indexIncrement>0) {
index+=indexIncrement;
QFile dum(dir+"/"+goodFiles->at(index));
if (dum.exists()) {
#ifdef Q_OS_WIN32
target.image=cv::imread(dum.fileName().toStdString().c_str());
#else
target.image=cv::imread(dum.fileName().toUtf8().data());
#endif
target.seqNumber=index;
return true;
} else {
qDebug()<<"File did not exist"<<dum.fileName();
}
} else {
return true; // it is not a real error after all
}
return false;
}
int ludcmp(Array2 <complex < doublevar> > & a, const int n,
Array1 <int> & indx, doublevar & d)
{
const doublevar TINY=1.0e-20;
// cout << "ludcmp" << endl;
Array1 <doublevar> vv(n);
//cout << "start " << endl;
d=1.0;
for (int i=0;i<n;++i)
{
doublevar big=0.0;
for (int j=0;j<n;++j)
{
//doublevar temp;
//if ((temp=fabs(a(i,j))) > big) big=temp;
doublevar temp=cabs(a(i,j));
if(temp>big)
big=temp;
}
if (big == 0.0)
return 0;//error("Singular matrix in routine ludcmp");
vv[i]=1.0/big;
}
//cout << "middle " << endl;
for (int j=0;j<n;++j)
{
int imax;
//cout << "j " << j << endl;
for (int i=0;i<j;++i)
{
//cout << "1i " << i << endl;
complex <double> sum=a(i,j);
for (int k=0;k<i;++k)
{
sum -= a(i,k)*a(k,j);
}
a(i,j)=sum;
}
//cout << "imax " << imax << endl;
doublevar big(0.0);
for (int i=j;i<n;++i)
{
//cout << " i " << i << endl;
complex < double> sum=a(i,j);
for (int k=0;k<j;++k)
sum -= a(i,k)*a(k,j);
a(i,j)=sum;
//doublevar dum;
//cout << "dum part " << endl;
//if ( (dum=vv[i]*fabs(sum)) >= big) {
doublevar dum=vv[i]*cabs(sum);
if(dum >=big)
{
big=dum;
imax=i;
}
}
//cout << "3 " << endl;
if (j != imax)
{
for (int k=0;k<n;++k)
{
complex <double> dum(a(imax,k));
a(imax,k)=a(j,k);
a(j,k)=dum;
}
d = -d;
vv[imax]=vv[j];
}
indx[j]=imax;
if (a(j,j) == 0.0)
{
a(j,j)=TINY;
//Write(n);
for (int q=0;q<n;++q)
for (int qq=0;qq<n;++qq)
//Write3(q,qq,a(q,qq));
//error("Singular matrix in routine ludcmp II.");
return 0;
}
//cout << "5 " << endl;
if (j != n-1)
{
complex < double> dum=1.0/(a(j,j));
for (int i=j+1;i<n;++i)
a(i,j) *= dum;
}
}
return 1;
}
FrameSource::IntrinsicParameters CameraV2::getIntrinsicParameters(void)
{
/* Assemble the name of the intrinsic parameter file: */
std::string intrinsicParameterFileName=KINECT_INTERNAL_CONFIG_CONFIGDIR;
intrinsicParameterFileName.push_back('/');
intrinsicParameterFileName.append(KINECT_INTERNAL_CONFIG_CAMERA_INTRINSICPARAMETERSFILENAMEPREFIX);
intrinsicParameterFileName.push_back('-');
intrinsicParameterFileName.append(serialNumber);
intrinsicParameterFileName.append(".dat");
/* Check if a file of the given name exists and is readable: */
IntrinsicParameters result;
if(IO::Directory::getCurrent()->getPathType(intrinsicParameterFileName.c_str())==Misc::PATHTYPE_FILE)
{
try
{
/* Open the parameter file: */
IO::FilePtr parameterFile(IO::Directory::getCurrent()->openFile(intrinsicParameterFileName.c_str()));
parameterFile->setEndianness(Misc::LittleEndian);
/* Read lens distortion correction parameters: */
Misc::Float64 depthLensDistortionParameters[5];
parameterFile->read(depthLensDistortionParameters,5);
for(int i=0;i<3;++i)
result.depthLensDistortion.setKappa(i,depthLensDistortionParameters[i]);
for(int i=0;i<2;++i)
result.depthLensDistortion.setRho(i,depthLensDistortionParameters[3+i]);
/* Read the depth unprojection matrix: */
Misc::Float64 depthMatrix[16];
parameterFile->read(depthMatrix,4*4);
result.depthProjection=IntrinsicParameters::PTransform::fromRowMajor(depthMatrix);
/* Read the color projection matrix: */
Misc::Float64 colorMatrix[16];
parameterFile->read(colorMatrix,4*4);
result.colorProjection=IntrinsicParameters::PTransform::fromRowMajor(colorMatrix);
}
catch(std::runtime_error err)
{
/* Log an error: */
Misc::formattedConsoleError("Kinect::CameraV2::getIntrinsicParameters: Could not load intrinsic parameter file %s due to exception %s",intrinsicParameterFileName.c_str(),err.what());
}
}
else
{
/* Get depth camera parameters from command dispatcher: */
const KinectV2CommandDispatcher::DepthCameraParams& dcp=commandDispatcher->getDepthCameraParams();
/* Initialize the lens distortion correction formula: */
result.depthLensDistortion.setKappa(0,dcp.k1);
result.depthLensDistortion.setKappa(1,dcp.k2);
result.depthLensDistortion.setKappa(2,dcp.k3);
result.depthLensDistortion.setRho(0,dcp.p1);
result.depthLensDistortion.setRho(1,dcp.p2);
/* Initialize the depth unprojection matrix: */
FrameSource::IntrinsicParameters::PTransform::Matrix& dum=result.depthProjection.getMatrix();
dum=FrameSource::IntrinsicParameters::PTransform::Matrix::zero;
dum(0,0)=-1.0/dcp.sx;
dum(0,3)=dcp.cx/dcp.sx;
dum(1,1)=1.0/dcp.sy;
dum(1,3)=-dcp.cy/dcp.sy;
dum(2,3)=-1.0;
dum(3,2)=-1.0/depthStreamReader->getA();
dum(3,3)=depthStreamReader->getB()/depthStreamReader->getA();
/* Scale the depth unprojection matrix to cm: */
result.depthProjection.leftMultiply(FrameSource::IntrinsicParameters::PTransform::scale(0.1));
/* Initialize the color projection matrix: */
result.colorProjection=FrameSource::IntrinsicParameters::PTransform::identity;
#if 1 // Evil temporary hack
IO::FilePtr colorMatrixFile=IO::Directory::getCurrent()->openFile("/home/okreylos/Projects/Kinect/ColorMatrix.dat");
colorMatrixFile->setEndianness(Misc::LittleEndian);
double colorMatrix[16];
colorMatrixFile->read(colorMatrix,16);
#if 0
for(int i=0;i<4;++i)
colorMatrix[i]=(colorMatrix[i+12]-colorMatrix[i])*1920.0;
for(int i=4;i<8;++i)
colorMatrix[i]*=1080.0;
#endif
for(int i=4;i<8;++i)
colorMatrix[i]=(colorMatrix[i+8]-colorMatrix[i]);
result.colorProjection=FrameSource::IntrinsicParameters::PTransform::fromRowMajor(colorMatrix);
result.colorProjection*=result.depthProjection;
/* Write the constructed calibration data to a calibration data file: */
std::cout<<"Writing intrinsic calibration parameters to file "<<intrinsicParameterFileName<<std::endl;
IO::FilePtr parameterFile(IO::Directory::getCurrent()->openFile(intrinsicParameterFileName.c_str(),IO::File::WriteOnly));
parameterFile->setEndianness(Misc::LittleEndian);
/* Write lens distortion correction parameters: */
for(int i=0;i<3;++i)
parameterFile->write<Misc::Float64>(result.depthLensDistortion.getKappa(i));
for(int i=0;i<2;++i)
parameterFile->write<Misc::Float64>(result.depthLensDistortion.getRho(i));
/* Write the depth unprojection matrix: */
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
parameterFile->write<Misc::Float64>(result.depthProjection.getMatrix()(i,j));
/* Write the color projection matrix: */
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
parameterFile->write<Misc::Float64>(result.colorProjection.getMatrix()(i,j));
#endif
}
return result;
}
