ModelTracker::Transform ModelTracker::position(const ModelTracker::ImgPoint imagePoints[],const ModelTracker::Transform::Rotation& orientation) const
{
/* Build the least-squares linear system: */
Math::Matrix ata(3,3,0.0);
Math::Matrix atb(3,1,0.0);
const Projection::Matrix& pm=projection.getMatrix();
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
{
/* Transform the model point with the known orientation and projection matrix: */
Projection::HVector pmp=projection.transform(Projection::HVector(orientation.transform(modelPoints[mpi])));
/* Create the two equations for the model/image point pair: */
double eq[2][4];
for(int i=0;i<2;++i)
{
for(int j=0;j<3;++j)
eq[i][j]=pm(i,j)-imagePoints[mpi][i]*pm(3,j);
eq[i][3]=imagePoints[mpi][i]*pmp[3]-pmp[i];
}
/* Add the two equations to the least-squares system: */
for(int i=0;i<3;++i)
{
for(int j=0;j<3;++j)
ata(i,j)+=eq[0][i]*eq[0][j]+eq[1][i]*eq[1][j];
atb(i)+=eq[0][i]*eq[0][3]+eq[1][i]*eq[1][3];
}
}
/* Solve the least-squares system: */
Math::Matrix x=atb.divideFullPivot(ata);
/* Return the result transformation: */
return Transform(Transform::Vector(x(0),x(1),x(2)),orientation);
}
void DepthCorrectionTool::buttonCallback(int buttonSlotIndex,Vrui::InputDevice::ButtonCallbackData* cbData)
{
if(cbData->newButtonState)
{
if(buttonSlotIndex==0)
{
/* Add a new averaged depth frame and calculate the best-fitting plane: */
DepthFrame df;
df.frame=Kinect::FrameBuffer(application->depthFrameSize[0],application->depthFrameSize[1],application->depthFrameSize[0]*application->depthFrameSize[1]*sizeof(float));
float foregroundCutoff=float(application->averageNumFrames)*0.5f;
float* afdPtr=application->averageFrameDepth;
float* affPtr=application->averageFrameForeground;
float* dfPtr=static_cast<float*>(df.frame.getBuffer());
typedef Geometry::PCACalculator<3>::Point PPoint;
typedef Geometry::PCACalculator<3>::Vector PVector;
Geometry::PCACalculator<3> pca;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++afdPtr,++affPtr,++dfPtr)
{
if(*affPtr>=foregroundCutoff)
{
/* Calculate the average depth value: */
*dfPtr=(*afdPtr)/(*affPtr);
/* Add the depth pixel to the PCA calculator: */
pca.accumulatePoint(PPoint(double(x)+0.5,double(y)+0.5,double(*dfPtr)));
}
else
*dfPtr=2047.0f;
}
/* Calculate the best-fitting plane: */
PPoint centroid=pca.calcCentroid();
pca.calcCovariance();
double evs[3];
pca.calcEigenvalues(evs);
PVector normal=pca.calcEigenvector(evs[2]);
df.plane=Plane(normal,centroid);
depthFrames.push_back(df);
}
else
{
/* Calculate the per-pixel affine depth correction coefficients: */
Kinect::FrameSource::PixelDepthCorrection* coefficients=new Kinect::FrameSource::PixelDepthCorrection[application->depthFrameSize[1]*application->depthFrameSize[0]];
Kinect::FrameSource::PixelDepthCorrection* cPtr=coefficients;
unsigned int pixelOffset=0;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
{
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++cPtr,++pixelOffset)
{
/* Build the least-squares linear regression system: */
Math::Matrix ata(2,2,0.0);
Math::Matrix atb(2,1,0.0);
unsigned int numFrames=0;
for(std::vector<DepthFrame>::iterator dfIt=depthFrames.begin();dfIt!=depthFrames.end();++dfIt)
{
double actual=double(static_cast<float*>(dfIt->frame.getBuffer())[pixelOffset]);
if(actual!=2047.0)
{
ata(0,0)+=actual*actual;
ata(0,1)+=actual;
ata(1,0)+=actual;
ata(1,1)+=1.0;
double expected=(dfIt->plane.getOffset()-(double(x)+0.5)*dfIt->plane.getNormal()[0]-(double(y)+0.5)*dfIt->plane.getNormal()[1])/dfIt->plane.getNormal()[2];
atb(0)+=actual*expected;
atb(1)+=expected;
++numFrames;
}
}
if(numFrames>=2)
{
/* Solve for the regression coefficients: */
Math::Matrix x=atb/ata;
cPtr->scale=float(x(0));
cPtr->offset=float(x(1));
}
else
{
/* Use identity correction if the pixel is underdetermined: */
cPtr->scale=1.0f;
cPtr->offset=0.0f;
}
}
}
/* Save the depth correction image: */
std::string depthCorrectionFileName=KINECT_CONFIG_DIR;
depthCorrectionFileName.push_back('/');
depthCorrectionFileName.append(KINECT_CAMERA_DEPTHCORRECTIONFILENAMEPREFIX);
depthCorrectionFileName.push_back('-');
depthCorrectionFileName.append(application->camera->getSerialNumber());
depthCorrectionFileName.append(".dat");
std::cout<<"Writing depth correction file "<<depthCorrectionFileName<<std::endl;
IO::FilePtr depthCorrectionFile(Vrui::openFile(depthCorrectionFileName.c_str(),IO::File::WriteOnly));
depthCorrectionFile->setEndianness(Misc::LittleEndian);
cPtr=coefficients;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++cPtr)
{
depthCorrectionFile->write<float>(cPtr->scale);
depthCorrectionFile->write<float>(cPtr->offset);
}
/* Clean up: */
delete[] coefficients;
}
}
}
ModelTracker::Transform ModelTracker::posit(ModelTracker::ImgPoint imagePoints[],unsigned int maxNumIterations)
{
/* Pre-transform the image points by the image transformation: */
for(unsigned int ipi=0;ipi<numModelPoints;++ipi)
imagePoints[ipi]=imgTransform.transform(imagePoints[ipi]);
/* Assign initial homogeneous weights to the model points: */
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
mpws[mpi]=1.0;
/* Iterate until convergence: */
Vector r1,r2,t;
for(unsigned int iteration=0;iteration<maxNumIterations;++iteration)
{
/*******************************************************************
Estimate the object's pose by approximating the perspective
projection with a scaled orthographic projection:
*******************************************************************/
/* Build the least-squares linear system: */
Math::Matrix atb(4,2,0.0);
for(unsigned int pi=0;pi<numModelPoints;++pi)
{
/* Right-hand side matrix contains image-space point positions multiplied by estimated homogeneous weights: */
for(int i=0;i<3;++i)
for(int j=0;j<2;++j)
atb(i,j)+=modelPoints[pi][i]*imagePoints[pi][j]*mpws[pi];
for(int j=0;j<2;++j)
atb(3,j)+=imagePoints[pi][j]*mpws[pi];
}
/* Solve the least-squares linear system: */
Math::Matrix x=invModelMat*atb;
/* Calculate the scale factor and the full rotation matrix and translation vector: */
for(int i=0;i<3;++i)
{
r1[i]=x(i,0);
r2[i]=x(i,1);
}
double s1=r1.mag();
double s2=r2.mag();
/* Orthogonalize the orientation vectors with minimum displacement: */
Vector r3=Geometry::normalize(r1^r2);
Vector mid=r1/s1+r2/s2;
mid/=mid.mag()*Math::sqrt(2.0);
Vector mid2=r3^mid;
r1=mid-mid2;
r2=mid+mid2;
double s=Math::sqrt(s1*s2);
t[0]=x(3,0)/s;
t[1]=x(3,1)/s;
t[2]=-f/s;
/* Update the homogeneous weights of the object points: */
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
mpws[mpi]=(r3*modelPoints[mpi])/t[2]+1.0;
}
/* Return the result transformation: */
return Transform(t,Geometry::invert(Transform::Rotation::fromBaseVectors(r1,r2)));
}
