//--------------------------------------------------------------
void ofxMtlMapping2DShape::update()
{
_super::update();
if(activePolygon != this && activePolygon != inputPolygon)
return;
if(activePolygon == this || (activePolygon == inputPolygon && inputPolygon) || (activePolygon == this && !inputPolygon && shapeType == MAPPING_2D_SHAPE_MASK)) {
setAsActiveShape();
// ---- recalculate the homography transformation matrix (for textured Shapes - for now only Quads) .
calcHomography();
}
if (activeShape == this) {
if (ofxMtlMapping2DControls::mapping2DControls()->mappingMode() == MAPPING_MODE_INPUT) {
if (shapeType == MAPPING_2D_SHAPE_QUAD) {
inputPolygon->update(true);
} else {
inputPolygon->update(false);
}
}
}
}
void Calibrador::addPts(ofVec2f ptS, ofVec2f ptD) {
// Referencia desde un punto de la imagen, no de la pantalla.
// ptsSrc: los de la Dcha
// ptsDst: los de la Izda
ptsSrc.push_back(ptS);
ptsDst.push_back(ptD);
if(ptsSrc.size()>=4) {
calcHomography();
}
}
//--------------------------------------------------------------
void mtl2dMappingShape::update()
{
_super::update();
if(activePolygon != this && activePolygon != inputPolygon)
return;
if(activePolygon == this || activePolygon == inputPolygon) {
setAsActiveShape();
}
if (activeShape == this) {
if (ControlsMapping::controlsMapping()->mappingMode() == MAPPING_MODE_INPUT) {
inputPolygon->update();
}
// ---- recalculate the homography transformation matrix.
calcHomography();
}
}
//--------------------------------------------------------------
void mtl2dMappingShape::init(int sId, bool defaultShape)
{
_super::init(sId, defaultShape);
calcHomography();
}
//--------------------------------------------------------------
void ofxMtlMapping2DShape::init(int sId, bool defaultShape)
{
_super::init(sId, defaultShape);
calcHomography();
}
ScreenCalibrator::ScreenCalibrator(int& argc,char**& argv,char**& appDefaults)
:Vrui::Application(argc,argv,appDefaults),
trackingPointsMover(0)
{
/* Create and register the point query tool class: */
PointQueryToolFactory* pointQueryToolFactory=new PointQueryToolFactory("PointQueryTool","Point Query",0,*Vrui::getToolManager());
pointQueryToolFactory->setNumButtons(1);
pointQueryToolFactory->setButtonFunction(0,"Query Point");
Vrui::getToolManager()->addClass(pointQueryToolFactory,Vrui::ToolManager::defaultToolFactoryDestructor);
/* Parse the command line: */
const char* optitrackFileName=0;
bool optitrackFlipZ=false;
const char* totalstationFileName=0;
int screenPixelSize[2]={-1,-1};
int screenSquareSize=200;
double unitScale=1.0;
for(int i=1;i<argc;++i)
{
if(argv[i][0]=='-')
{
if(strcasecmp(argv[i]+1,"screenSize")==0)
{
for(int j=0;j<2;++j)
{
++i;
screenPixelSize[j]=atoi(argv[i]);
}
}
else if(strcasecmp(argv[i]+1,"squareSize")==0)
{
++i;
screenSquareSize=atoi(argv[i]);
}
else if(strcasecmp(argv[i]+1,"metersToInches")==0)
unitScale=1000.0/25.4;
else if(strcasecmp(argv[i]+1,"unitScale")==0)
{
++i;
unitScale=atof(argv[i]);
}
else if(strcasecmp(argv[i]+1,"flipZ")==0)
optitrackFlipZ=true;
else
{
}
}
else if(totalstationFileName==0)
totalstationFileName=argv[i];
else if(optitrackFileName==0)
optitrackFileName=argv[i];
else
{
}
}
/* Read the Optitrack sample file: */
if(optitrackFileName!=0)
{
readOptitrackSampleFile(optitrackFileName,optitrackFlipZ);
std::cout<<"Read "<<trackingPoints.size()<<" ball points from Optitrack sample file"<<std::endl;
}
/* Read relevant point classes from the Totalstation survey file: */
if(totalstationFileName!=0)
{
screenPoints=readTotalstationSurveyFile(totalstationFileName,"SCREEN");
floorPoints=readTotalstationSurveyFile(totalstationFileName,"FLOOR");
ballPoints=readTotalstationSurveyFile(totalstationFileName,"BALLS");
std::cout<<"Read "<<ballPoints.size()<<" ball points from TotalStation survey file"<<std::endl;
}
/*********************************************************************
Establish a normalized coordinate system with the floor at the z=0
plane, the screen in a plane about orthogonal to the y axis, and the
screen center above the origin.
*********************************************************************/
/* Fit a plane to the floor points: */
Geometry::PCACalculator<3> floorPca;
for(PointList::const_iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
floorPca.accumulatePoint(*fpIt);
Point floorCentroid=floorPca.calcCentroid();
floorPca.calcCovariance();
double floorEv[3];
floorPca.calcEigenvalues(floorEv);
Geometry::PCACalculator<3>::Vector floorNormal=floorPca.calcEigenvector(floorEv[2]);
/* Fit a plane to the screen points: */
Geometry::PCACalculator<3> screenPca;
for(PointList::const_iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
screenPca.accumulatePoint(*spIt);
Point screenCentroid=screenPca.calcCentroid();
screenPca.calcCovariance();
double screenEv[3];
screenPca.calcEigenvalues(screenEv);
Geometry::PCACalculator<3>::Vector screenNormal=screenPca.calcEigenvector(screenEv[2]);
/* Flip the floor normal such that it points towards the screen points: */
if((screenCentroid-floorCentroid)*floorNormal<Scalar(0))
floorNormal=-floorNormal;
/* Flip the screen normal such that it points away from the ball points: */
Point::AffineCombiner ballC;
for(PointList::const_iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
ballC.addPoint(*bpIt);
if((ballC.getPoint()-screenCentroid)*screenNormal>Scalar(0))
screenNormal=-screenNormal;
/* Project the screen centroid onto the floor plane to get the coordinate system origin: */
Point origin=screenCentroid-floorNormal*(((screenCentroid-floorCentroid)*floorNormal)/Geometry::sqr(floorNormal));
/* Orthonormalize the screen normal against the floor normal: */
Vector y=screenNormal-floorNormal*((screenNormal*floorNormal)/Geometry::sqr(floorNormal));
Vector x=Geometry::cross(y,floorNormal);
#if 0
/* Calculate a rotation to align the floor normal with +z and the (horizontal) screen normal with +y: */
ONTransform::Rotation rot=ONTransform::Rotation::fromBaseVectors(x,y);
#endif
/*********************************************************************
Calculate a transformation to move the Totalstation survey points into
the normalized coordinate system:
*********************************************************************/
ONTransform transform(origin-Point::origin,ONTransform::Rotation::fromBaseVectors(x,y));
transform.doInvert();
/* Transform all survey points: */
for(PointList::iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
*spIt=transform.transform(*spIt);
for(PointList::iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
*fpIt=transform.transform(*fpIt);
for(PointList::iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
*bpIt=transform.transform(*bpIt);
if(screenPixelSize[0]>0&&screenPixelSize[1]>0&&screenSquareSize>0)
{
/*********************************************************************
Calculate the optimal projective transformation and screen
transformation (orthonormal transformation plus non-uniform scaling in
x and y) from theoretical screen points to surveyed screen points:
*********************************************************************/
/* Create a list of theoretical screen points: */
PointList screen;
int screenPixelOffset[2];
for(int i=0;i<2;++i)
screenPixelOffset[i]=((screenPixelSize[i]-1)%screenSquareSize)/2;
for(int y=screenPixelOffset[1];y<screenPixelSize[1];y+=screenSquareSize)
for(int x=screenPixelOffset[0];x<screenPixelSize[0];x+=screenSquareSize)
screen.push_back(Point((Scalar(x)+Scalar(0.5))/Scalar(screenPixelSize[0]),Scalar(1)-(Scalar(y)+Scalar(0.5))/Scalar(screenPixelSize[1]),0));
if(screen.size()!=screenPoints.size())
Misc::throwStdErr("Wrong number of screen points, got %d instead of %d",int(screenPoints.size()),int(screen.size()));
/* Find the best-fitting projective transformation for the measured screen points: */
PTransformFitter ptf(screen.size(),&screen[0],&screenPoints[0]);
PTransformFitter::Scalar screenResult2=LevenbergMarquardtMinimizer<PTransformFitter>::minimize(ptf);
std::cout<<"Projective transformation fitting final distance: "<<screenResult2<<std::endl;
pScreenTransform=ptf.getTransform();
/* Print the screen transformation matrix: */
std::cout<<"Projective transformation matrix:"<<std::endl;
std::cout<<std::setprecision(6)<<pScreenTransform<<std::endl;
/* Find the best-fitting screen transformation for the measured screen points: */
ScreenTransformFitter stf(screen.size(),&screen[0],&screenPoints[0]);
ScreenTransformFitter::Scalar screenResult1=LevenbergMarquardtMinimizer<ScreenTransformFitter>::minimize(stf);
std::cout<<"Screen transformation fitting final distance: "<<screenResult1<<std::endl;
screenTransform=stf.getTransform();
screenSize[0]=stf.getSize(0);
screenSize[1]=stf.getSize(1);
std::cout<<"Optimal screen size: "<<screenSize[0]<<", "<<screenSize[1]<<std::endl;
std::cout<<"Optimal screen origin: "<<screenTransform.getOrigin()<<std::endl;
std::cout<<"Optimal horizontal screen axis: "<<screenTransform.getDirection(0)<<std::endl;
std::cout<<"Optimal vertical screen axis: "<<screenTransform.getDirection(1)<<std::endl;
/*********************************************************************
Calculate a homography matrix from the optimal screen transformation
to the optimal projective transformation to correct screen
misalignments:
*********************************************************************/
Point sCorners[4];
Point pCorners[4];
for(int i=0;i<4;++i)
{
sCorners[i][0]=i&0x1?screenSize[0]*unitScale:0.0;
sCorners[i][1]=i&0x2?screenSize[1]*unitScale:0.0;
sCorners[i][2]=0.0;
pCorners[i][0]=i&0x1?1.0:0.0;
pCorners[i][1]=i&0x2?1.0:0.0;
pCorners[i][2]=0.0;
pCorners[i]=screenTransform.inverseTransform(pScreenTransform.transform(pCorners[i]));
pCorners[i][0]*=unitScale;
pCorners[i][1]*=unitScale;
}
Geometry::ProjectiveTransformation<double,2> sHom=calcHomography(sCorners);
Geometry::ProjectiveTransformation<double,2> pHom=calcHomography(pCorners);
Geometry::ProjectiveTransformation<double,2> hom=pHom;
hom.leftMultiply(Geometry::invert(sHom));
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
hom.getMatrix()(i,j)/=hom.getMatrix()(2,2);
#if 0
std::cout<<"Homography matrix for projective transform: "<<pHom<<std::endl;
std::cout<<"Homography matrix for screen transform: "<<sHom<<std::endl;
std::cout<<"Screen correction homography matrix: "<<hom<<std::endl;
#endif
#if 0
/* Do some experiments: */
Geometry::ProjectiveTransformation<double,3> hom3=Geometry::ProjectiveTransformation<double,3>::identity;
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
hom3.getMatrix()(i<2?i:3,j<2?j:3)=hom.getMatrix()(i,j);
hom3.getMatrix()(2,0)=hom3.getMatrix()(3,0);
hom3.getMatrix()(2,1)=hom3.getMatrix()(3,1);
std::cout<<hom3<<std::endl;
std::cout<<Geometry::invert(hom3)<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0,-1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0,-1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0, 1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0, 1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0,-1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0,-1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0, 1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0, 1.0, 1.0,1.0)).toPoint()<<std::endl;
#endif
/* Print a configuration file section for the screen: */
std::cout<<std::endl<<"Configuration settings for screen:"<<std::endl;
std::cout<<"origin "<<screenTransform.getTranslation()*unitScale<<std::endl;
std::cout<<"horizontalAxis "<<screenTransform.getDirection(0)<<std::endl;
std::cout<<"width "<<screenSize[0]*unitScale<<std::endl;
std::cout<<"verticalAxis "<<screenTransform.getDirection(1)<<std::endl;
std::cout<<"height "<<screenSize[1]*unitScale<<std::endl;
std::cout<<"offAxis true"<<std::endl;
std::cout<<"homography ( ";
for(int j=0;j<3;++j)
{
if(j>0)
std::cout<<", \\"<<std::endl<<" ";
std::cout<<"( ";
for(int i=0;i<3;++i)
{
if(i>0)
std::cout<<", ";
std::cout<<pHom.getMatrix()(i,j);
}
std::cout<<" )";
}
std::cout<<" )"<<std::endl;
std::cout<<std::endl;
}
if(optitrackFileName!=0&&totalstationFileName!=0)
{
/*********************************************************************
Calculate the optimal orthonormal transformation from tracking system
coordinates to the normalized coordinate system by aligning ball
positions observed by the tracking system with ball positions measured
using the total station:
*********************************************************************/
/* Find an orthonormal transformation to align the tracking points with the ball points: */
size_t numPoints=trackingPoints.size();
if(numPoints>ballPoints.size())
numPoints=ballPoints.size();
/* Calculate the centroid of the tracking points: */
Point::AffineCombiner tpCc;
for(size_t i=0;i<numPoints;++i)
tpCc.addPoint(trackingPoints[i]);
Vector tpTranslation=tpCc.getPoint()-Point::origin;
for(size_t i=0;i<numPoints;++i)
trackingPoints[i]-=tpTranslation;
ONTransformFitter ontf(numPoints,&trackingPoints[0],&ballPoints[0]);
//ontf.setTransform(ONTransformFitter::Transform::rotate(ONTransformFitter::Transform::Rotation::rotateX(Math::rad(Scalar(90)))));
ONTransformFitter::Scalar result=LevenbergMarquardtMinimizer<ONTransformFitter>::minimize(ontf);
ONTransform tsCal=ontf.getTransform();
tsCal*=ONTransform::translate(-tpTranslation);
std::cout<<"Final distance: "<<result<<std::endl;
std::cout<<"Tracking system calibration transformation: "<<tsCal<<std::endl;
std::cout<<"Configuration settings for tracking calibrator: "<<std::endl;
std::cout<<"transformation translate "<<tsCal.getTranslation()*unitScale<<" \\"<<std::endl;
std::cout<<" * scale "<<unitScale<<" \\"<<std::endl;
std::cout<<" * rotate "<<tsCal.getRotation().getAxis()<<", "<<Math::deg(tsCal.getRotation().getAngle())<<std::endl;
/* Transform the tracking points with the result transformation: */
for(PointList::iterator tpIt=trackingPoints.begin();tpIt!=trackingPoints.end();++tpIt)
*tpIt=tsCal.transform(*tpIt+tpTranslation);
}
/* Initialize the navigation transformation: */
Geometry::Box<Scalar,3> bbox=Geometry::Box<Scalar,3>::empty;
for(PointList::const_iterator tpIt=trackingPoints.begin();tpIt!=trackingPoints.end();++tpIt)
bbox.addPoint(*tpIt);
for(PointList::const_iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
bbox.addPoint(*spIt);
for(PointList::const_iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
bbox.addPoint(*fpIt);
for(PointList::const_iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
bbox.addPoint(*bpIt);
Vrui::setNavigationTransformation(Geometry::mid(bbox.min,bbox.max),Geometry::dist(bbox.min,bbox.max));
/* Create a virtual input device to move the tracking points interactively: */
trackingPointsMover=Vrui::addVirtualInputDevice("TrackingPointsMover",0,0);
// Vrui::getInputGraphManager()->setNavigational(trackingPointsMover,true);
Vrui::NavTrackerState scaledDeviceT=Vrui::getInverseNavigationTransformation();
scaledDeviceT*=trackingPointsMover->getTransformation();
trackingPointsTransform=Vrui::TrackerState(scaledDeviceT.getTranslation(),scaledDeviceT.getRotation());
trackingPointsTransform.doInvert();
}