void BlockPhysicsComponent::rasterize(Polygon2 const &polygon)
{
Polygon2 localPolygon;
for (int i = 0; i < polygon.getSize(); ++i) {
b2Vec2 worldPoint(polygon.vertices[i].x, polygon.vertices[i].y);
b2Vec2 localPoint = body_->GetLocalPoint(worldPoint);
localPolygon.vertices.push_back(Vector2(localPoint.x, localPoint.y));
}
Box2 bounds = localPolygon.getBoundingBox();
int minX = int(10.0f * bounds.p1.x + 0.05f);
int minY = int(10.0f * bounds.p1.y + 0.05f);
int maxX = int(10.0f * bounds.p2.x + 0.05f);
int maxY = int(10.0f * bounds.p2.y + 0.05f);
for (int y = minY; y <= maxY; ++y) {
for (int x = minX; x <= maxX; ++x) {
Vector2 localPoint(0.1f * float(x), 0.1f * float(y));
if (localPolygon.containsPoint(localPoint)) {
for (int dy = -1; dy <= 1; ++dy) {
for (int dx = -1; dx <= 1; ++dx) {
if (dx == 0 || dy == 0) {
setElement(x + dx, y + dy, 1);
}
}
}
}
}
}
}
void NurbsCurveEmitter::Fill(const FieldRef& field)
{
MFnNurbsCurve curve(mObject);
// Get the range for U and V.
MPlug minValuePlug = curve.findPlug("minValue");
MPlug maxValuePlug = curve.findPlug("maxValue");
const Double minValue = minValuePlug.asDouble();
const Double maxValue = maxValuePlug.asDouble();
const Double valueRange = maxValue - minValue;
int i = 0;
for (i = 0; i < static_cast<int>(mSample); ++ i)
{
double u = Stokes::Random::NextAsDouble();
double v = Stokes::Random::NextAsDouble();
double w = Stokes::Random::NextAsDouble();
double param = u * valueRange + minValue;
MPoint p;
curve.getPointAtParam(param, p, MSpace::kWorld);
MVector t = curve.tangent(param, MSpace::kWorld);
t.normalize();
MVector n = curve.normal(param, MSpace::kWorld);
n.normalize();
MVector b = n ^ t;
double r = sqrt(v);
double phi = w * 2.0 * M_PI;
double x = r * cos(phi);
double y = r * sin(phi);
// TODO: No radius here.
MPoint newP = p + n * x + b * y;
MVector radialDirection = newP - p;
radialDirection.normalize();
Stokes::Vectorf noisedPoint(Random::NextAsFloat() * mScale.x - mOffset.x, Random::NextAsFloat() * mScale.y - mOffset.y, static_cast<Float>(u) * mScale.z - mOffset.z);
Float displacement = Stokes::Noiser::FractalBrownianMotion(noisedPoint, mDisplacedH, mDisplacedLacunarity, mDisplacedOctave) * mDisplacedAmplitude;
MPoint displacedP = newP + radialDirection * displacement;
Stokes::Vectorf worldPoint(static_cast<Stokes::Float>(displacedP.x), static_cast<Stokes::Float>(displacedP.y), static_cast<Stokes::Float>(displacedP.z));
Stokes::Vectoriu index;
if (field->CalculateIndexFromWorldPoint(worldPoint, index))
{
Float density = Stokes::Noiser::FractalBrownianMotion(noisedPoint, mH, mLacunarity, mOctave) * mAmplitude;
if (density > 0)
{
field->Access(index)[0] += density;
}
}
}
}
hkDemo::Result PrevailingWindDemo::stepDemo()
{
hkVector4 worldPoint( 0.0f, 8.0f, 0.0f);
hkVector4 windAtWorldPoint;
{
m_wind->getWindVector( worldPoint, windAtWorldPoint );
}
HK_DISPLAY_ARROW( worldPoint, windAtWorldPoint, hkColor::BLUE);
return hkDefaultPhysicsDemo::stepDemo();
}
QPixmap RectificationController::assistedFromProjectionToSimilarity(ClickableLabel* projectedImageLabel, const QSize& POISize,
bool pointOfInterestFlag)
{
// First, create pairs of point correlations between projection and world space.
CircularList<SelectedPixel*>* selectedPixelsProj = projectedImageLabel->getSelectedPixels();
int numSelectedPixels = selectedPixelsProj->size();
vector<QPoint> selectedPixelsWorld(numSelectedPixels);
selectedPixelsWorld[0] = QPoint(0, 0);
selectedPixelsWorld[1] = selectedPixelsWorld[0] + QPoint(0, POISize.height());
selectedPixelsWorld[2] = selectedPixelsWorld[0] + QPoint(POISize.width(), POISize.height());
selectedPixelsWorld[3] = selectedPixelsWorld[0] + QPoint(POISize.width(), 0);
vector<pair<VectorXd, VectorXd>> correlationPoints(numSelectedPixels);
for (int i = 0; i < numSelectedPixels; ++i)
{
QPoint qProjectedPoint = (*selectedPixelsProj)[i]->getPos();
QPoint qWorldPoint = selectedPixelsWorld[i];
VectorXd projectedPoint(3);
projectedPoint << qProjectedPoint.x(), qProjectedPoint.y(), 1.;
VectorXd worldPoint(3);
worldPoint << qWorldPoint.x(), qWorldPoint.y(), 1.;
pair<VectorXd, VectorXd> correlationPair(projectedPoint, worldPoint);
correlationPoints[i] = correlationPair;
}
// Second, define the projection to world transformation.
AssistedSimilarityFromProjRectificator rectificator(correlationPoints);
MatrixXd projToWorld = *rectificator.getTransformation();
// Qt uses the transpose of the usual transformation representation.
QTransform qProjToWorld(
projToWorld(0, 0), projToWorld(1, 0), projToWorld(2, 0),
projToWorld(0, 1), projToWorld(1, 1), projToWorld(2, 1),
projToWorld(0, 2), projToWorld(1, 2), projToWorld(2, 2)
);
QPixmap rectifiedPixmap = projectedImageLabel->pixmap()->transformed(qProjToWorld, Qt::SmoothTransformation);
if (pointOfInterestFlag)
{
return selectPointOfInterest(rectifiedPixmap, qProjToWorld, projectedImageLabel->pixmap()->size(), (*selectedPixelsProj)[0]->getPos(), POISize);
}
else
{
return rectifiedPixmap;
}
}
/// Use the active window and matrices to get a world position from a screen coordinate
Vector2D ScreenToWorld2D(Vector2<int> point)
{
Vector2D worldPoint(0.f, 0.f);
Window* windowPtr = GraphicsDevice::instance()->getWindow();
mat4 cameraMatrix = GraphicsDevice::instance()->getProjectionMatrix() * GraphicsDevice::instance()->getViewMatrix();
if (windowPtr)
{
Vector2D h**o = windowPtr->convertToHomogeneousCoordinate(point);
Vector4D worldCoordinate = cameraMatrix.inverse() * Vector4D(h**o.x, h**o.y, 0.f, 1.f);
worldPoint.x = worldCoordinate.x;
worldPoint.y = worldCoordinate.y;
}
return worldPoint;
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
btVector3 pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
transforms[i].setOrigin( pos );
btQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (!m_initialized)
{
m_initialized = true;
glGenTextures(1, &glTextureId);
}
glBindTexture(GL_TEXTURE_2D,glTextureId );
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
btVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
btVector3 rayFrom = getCameraPosition();
btVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 vertical(0.f,1.f,0.f);
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(screenWidth);
btVector3 dVert = vertical * 1.f/float(screenHeight);
btTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
btTransform rayFromLocal;
btTransform rayToLocal;
int x;
///clear texture
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
btVector4 rgba(0.2f,0.2f,0.2f,1.f);
raytracePicture->setPixel(x,y,rgba);
}
}
#if 1
btVector3 rayTo;
btTransform colObjWorldTransform;
colObjWorldTransform.setIdentity();
int mode = 0;
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
btVector3 worldNormal(0,0,0);
btVector3 worldPoint(0,0,0);
bool hasHit = false;
int mode = 0;
switch (mode)
{
case 0:
hasHit = lowlevelRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 1:
hasHit = singleObjectRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 2:
hasHit = worldRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
default:
{
}
}
if (hasHit)
{
float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f));
float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f));
rgba = btVector4(lightVec0,lightVec1,0,1.f);
rgba.setMin(btVector3(1,1,1));
rgba.setMax(btVector3(0.2,0.2,0.2));
rgba[3] = 1.f;
raytracePicture->setPixel(x,y,rgba);
} else
btVector4 rgba = raytracePicture->getPixel(x,y);
if (!rgba.length2())
{
raytracePicture->setPixel(x,y,btVector4(1,1,1,1));
}
}
}
#endif
extern unsigned char sFontData[];
if (0)
{
const char* text="ABC abc 123 !@#";
int x=0;
for (int cc = 0;cc<strlen(text);cc++)
{
char testChar = text[cc];//'b';
char ch = testChar-32;
int startx=ch%16;
int starty=ch/16;
//for (int i=0;i<256;i++)
for (int i=startx*16;i<(startx*16+16);i++)
{
int y=0;
//for (int j=0;j<256;j++)
//for (int j=0;j<256;j++)
for (int j=starty*16;j<(starty*16+16);j++)
{
btVector4 rgba(0,0,0,1);
rgba[0] = (sFontData[i*3+255*256*3-(256*j)*3])/255.f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i)*3+255*256*3-(256*j+1)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j+1)*3])/255.*0.25;
//if (rgba[0]!=0.f)
{
rgba[1]=rgba[0];
rgba[2]=rgba[0];
rgba[3]=1.f;
//raytracePicture->setPixel(x,y,rgba);
raytracePicture->addPixel(x,y,rgba);
}
y++;
}
x++;
}
}
}
//raytracePicture->grapicalPrintf("CCD RAYTRACER",sFontData);
char buffer[256];
sprintf(buffer,"%d rays",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"Toggle",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"TEST",screenWidth*screenHeight*numObjects);
//raytracePicture->grapicalPrintf(buffer,sFontData,0,10);//&BMF_font_helv10,0,10);
raytracePicture->grapicalPrintf(buffer,sFontData,0,0);//&BMF_font_helv10,0,10);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.1f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->getBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->getWidth(),raytracePicture->getHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::drawCoordSystem();
{
for (int i=0;i<numObjects;i++)
{
btVector3 aabbMin,aabbMax;
shapePtr[i]->getAabb(transforms[i],aabbMin,aabbMax);
}
}
glPushMatrix();
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
m_azi += 1.f;
glFlush();
glutSwapBuffers();
}
void ITKImplicit::initFromSurfaceMesh(SurfaceMesh * mesh)
{
SignedDistanceTransform dt(*mesh);
//fill up voxel grid values
ImageType::SizeType size = myImage->GetLargestPossibleRegion().GetSize();
ImageType::PointType origin = myImage->GetOrigin();
ImageType::SpacingType spacing = myImage->GetSpacing();
ImageType::IndexType index;
for (unsigned int z=0;z<size[2];++z)
{
index[2]=z;
for (unsigned int y=0;y<size[1];++y)
{
index[1]=y;
for (unsigned int x=0;x<size[0];++x)
{
index[0]=x;
myImage->SetPixel(index,DBL_MAX);
}
}
}
for (unsigned int z=0;z<size[2];++z)
{
index[2]=z;
for (unsigned int y=0;y<size[1];++y)
{
index[1]=y;
for (unsigned int x=0;x<size[0];++x)
{
index[0]=x;
//calculate world position of voxel
gmVector3 worldPoint(origin[0]+x*spacing[0], origin[1]+y*spacing[1], origin[2]+z*spacing[2]);
myImage->SetPixel(index,dt.signedDistanceTo(worldPoint));
}
}
}
//Implementation using only the nearest point class
//std::vector<TriangleNearestPoint> distanceTester;
//std::vector<gmVector3> normals;
//for (SurfaceMesh::ConstFaceIter f_it = mesh->faces_begin(); f_it!=mesh->faces_end(); ++f_it)
//{
// SurfaceMesh::Point n(mesh->normal(f_it.handle()));
// normals.push_back(gmVector3(n[0],n[1],n[2]));
// std::vector<gmVector3> vertices;
// for (SurfaceMesh::ConstFaceVertexIter fv_it = mesh->cfv_iter(f_it.handle()); fv_it; ++fv_it)
// {
// SurfaceMesh::Point p= mesh->point(fv_it.handle());
// vertices.push_back(gmVector3(p[0],p[1],p[2]));
// }
// //test if mesh is triangular
// if (vertices.size()>3)
// return;
// assert(vertices.size()==3);
// distanceTester.push_back(TriangleNearestPoint(vertices[0],vertices[1],vertices[2]));
//}
////fill up voxel grid values
//ImageType::SizeType size = myImage->GetLargestPossibleRegion().GetSize();
//ImageType::PointType origin = myImage->GetOrigin();
//ImageType::SpacingType spacing = myImage->GetSpacing();
//TriangleNearestPoint::VoronoiRegion region;
//gmVector3 nearestPoint;
//ImageType::IndexType index;
////init image values to FLT_MAX
//for (unsigned int z=0;z<size[2];++z)
//{
// index[2]=z;
// for (unsigned int y=0;y<size[1];++y)
// {
// index[1]=y;
// for (unsigned int x=0;x<size[0];++x)
// {
// index[0]=x;
// myImage->SetPixel(index,DBL_MAX);
// }
// }
//}
//for (unsigned int z=0;z<size[2];++z)
//{
// index[2]=z;
// for (unsigned int y=0;y<size[1];++y)
// {
// index[1]=y;
// for (unsigned int x=0;x<size[0];++x)
// {
// index[0]=x;
//
// //calculate world position of voxel
// gmVector3 worldPoint(origin[0]+x*spacing[0], origin[1]+y*spacing[1], origin[2]+z*spacing[2]);
//
// for (unsigned int i=0;i<distanceTester.size();++i)
// {
// nearestPoint=distanceTester[i].NearestPointTo(worldPoint,region);
// gmVector3 diff=(worldPoint-nearestPoint);
// double value=diff.length();
// double oldValue=myImage->GetPixel(index);
// if (fabs(oldValue)>value)
// {
// value=(dot(diff,normals[i])>0)?value:-value;
// myImage->SetPixel(index,value);
// }
// }
// }
// }
//}
}
void downloadCalibration(unsigned int index)
{
/* Open a Kinect camera: */
Kinect::Camera camera(usbContext,index);
std::cout<<"Downloading factory calibration data for Kinect "<<camera.getSerialNumber()<<"..."<<std::endl;
/* Retrieve the factory calibration data: */
Kinect::Camera::CalibrationParameters cal;
camera.getCalibrationParameters(cal);
/* Calculate the depth-to-distance conversion formula: */
double numerator=10.0*(4.0*cal.dcmosEmitterDist*cal.referenceDistance)/cal.referencePixelSize;
double denominator=4.0*cal.dcmosEmitterDist/cal.referencePixelSize+4.0*cal.constantShift+1.5;
std::cout<<"Depth conversion formula: dist[mm] = "<<numerator<<" / ("<<denominator<<" - depth)"<<std::endl;
/* Calculate the depth pixel unprojection matrix: */
double scale=2.0*cal.referencePixelSize/cal.referenceDistance;
Math::Matrix depthMatrix(4,4,0.0);
depthMatrix(0,0)=scale;
depthMatrix(0,3)=-scale*640.0/2.0;
depthMatrix(1,1)=scale;
depthMatrix(1,3)=-scale*480.0/2.0;
depthMatrix(2,3)=-1.0;
depthMatrix(3,2)=-1.0/numerator;
depthMatrix(3,3)=denominator/numerator;
{
std::cout<<std::endl<<"Depth unprojection matrix:"<<std::endl;
std::streamsize oldPrec=std::cout.precision(8);
std::cout.setf(std::ios::fixed);
for(int i=0;i<4;++i)
{
std::cout<<" ";
for(int j=0;j<4;++j)
std::cout<<' '<<std::setw(11)<<depthMatrix(i,j);
std::cout<<std::endl;
}
std::cout.unsetf(std::ios::fixed);
std::cout.precision(oldPrec);
}
/* Calculate the depth-to-color mapping: */
double colorShiftA=cal.dcmosRcmosDist/(cal.referencePixelSize*2.0)+0.375;
double colorShiftB=cal.dcmosRcmosDist*cal.referenceDistance*10.0/(cal.referencePixelSize*2.0);
std::cout<<"Depth-to-color pixel shift formula: Shift = "<<colorShiftA<<" - "<<colorShiftB<<"/dist[mm]"<<std::endl;
/* Formula to go directly from depth to displacement: */
double dispA=colorShiftA-colorShiftB*denominator/numerator;
double dispB=colorShiftB/numerator;
std::cout<<"Or: Shift = "<<dispA<<" + "<<dispB<<"*depth"<<std::endl;
/* Tabulate the bivariate pixel mapping polynomial using forward differencing: */
double* colorx=new double[640*480];
double* colory=new double[640*480];
double ax=cal.ax;
double bx=cal.bx;
double cx=cal.cx;
double dx=cal.dx;
double ay=cal.ay;
double by=cal.by;
double cy=cal.cy;
double dy=cal.dy;
double dx0=(cal.dxStart<<13)>>4;
double dy0=(cal.dyStart<<13)>>4;
double dxdx0=(cal.dxdxStart<<11)>>3;
double dxdy0=(cal.dxdyStart<<11)>>3;
double dydx0=(cal.dydxStart<<11)>>3;
double dydy0=(cal.dydyStart<<11)>>3;
double dxdxdx0=(cal.dxdxdxStart<<5)<<3;
double dydxdx0=(cal.dydxdxStart<<5)<<3;
double dxdxdy0=(cal.dxdxdyStart<<5)<<3;
double dydxdy0=(cal.dydxdyStart<<5)<<3;
double dydydx0=(cal.dydydxStart<<5)<<3;
double dydydy0=(cal.dydydyStart<<5)<<3;
for(int y=0;y<480;++y)
{
dxdxdx0+=cx;
dxdx0 +=dydxdx0/256.0;
dydxdx0+=dx;
dx0 +=dydx0/64.0;
dydx0 +=dydydx0/256.0;
dydydx0+=bx;
dxdxdy0+=cy;
dxdy0 +=dydxdy0/256.0;
dydxdy0+=dy;
dy0 +=dydy0/64.0;
dydy0 +=dydydy0/256.0;
dydydy0+=by;
double coldxdxdy=dxdxdy0;
double coldxdy=dxdy0;
double coldy=dy0;
double coldxdxdx=dxdxdx0;
double coldxdx=dxdx0;
double coldx=dx0;
for(int x=0;x<640;++x)
{
colorx[y*640+x]=double(x)+coldx/131072.0+1.0;
colory[y*640+x]=double(y)+coldy/131072.0+1.0;
#if 0
if(x%40==20&&y%40==20)
std::cout<<x<<", "<<y<<": "<<colorx[y*640+x]<<", "<<colory[y*640+x]<<std::endl;
#endif
coldx+=coldxdx/64.0;
coldxdx+=coldxdxdx/256.0;
coldxdxdx+=ax;
coldy+=coldxdy/64.0;
coldxdy+=coldxdxdy/256.0;
coldxdxdy+=ay;
}
}
/* Calculate the texture projection matrix by sampling the pixel mapping polynomial: */
int minDepth=Math::ceil(denominator-numerator/500.0); // 500mm is minimum viewing distance
int maxDepth=Math::ceil(denominator-numerator/3000.0); // 3000mm is maximum reasonable viewing distance
std::cout<<"Calculating best-fit color projection matrix for depth value range "<<minDepth<<" - "<<maxDepth<<"..."<<std::endl;
Math::Matrix colorSystem(12,12,0.0);
Math::Matrix depthPoint(4,1,0.0);
depthPoint(3)=1.0;
for(int y=20;y<480;y+=40)
{
depthPoint(1)=(double(y)+0.5)/480.0;
for(int x=20;x<640;x+=40)
{
depthPoint(0)=(double(x)+0.5)/640.0;
for(int depth=minDepth;depth<=maxDepth;depth+=20)
{
/* Transform the depth point to color image space using the non-linear transformation: */
// int xdisp=x+Math::floor(dispA+dispB*double(depth)+0.5);
// if(xdisp>=0&&xdisp<640)
{
// double colX=double(xdisp)/640.0;
// double colY=double(y)/480.0;
// double colX=colorx[(479-y)*640+xdisp]/640.0;
// double colY=(479.0-colory[(479-y)*640+xdisp])/480.0;
double colX=(colorx[(479-y)*640+x]+dispA+dispB*double(depth))/640.0;
double colY=(479.0-colory[(479-y)*640+x])/480.0;
/* Calculate the 3D point based on the depth unprojection matrix: */
depthPoint(2)=double(depth)/double(maxDepth);
Math::Matrix worldPoint=depthMatrix*depthPoint;
/* Make the world point affine: */
for(int i=0;i<3;++i)
worldPoint(i)/=worldPoint(3);
/* Enter the world point / color point pair into the color projection system: */
double eq[2][12];
eq[0][0]=depthPoint(0);
eq[0][1]=depthPoint(1);
eq[0][2]=depthPoint(2);
eq[0][3]=1.0;
eq[0][4]=0.0;
eq[0][5]=0.0;
eq[0][6]=0.0;
eq[0][7]=0.0;
eq[0][8]=-colX*depthPoint(0);
eq[0][9]=-colX*depthPoint(1);
eq[0][10]=-colX*depthPoint(2);
eq[0][11]=-colX;
eq[1][0]=0.0;
eq[1][1]=0.0;
eq[1][2]=0.0;
eq[1][3]=0.0;
eq[1][4]=depthPoint(0);
eq[1][5]=depthPoint(1);
eq[1][6]=depthPoint(2);
eq[1][7]=1.0;
eq[1][8]=-colY*depthPoint(0);
eq[1][9]=-colY*depthPoint(1);
eq[1][10]=-colY*depthPoint(2);
eq[1][11]=-colY;
for(int row=0;row<2;++row)
for(unsigned int i=0;i<12;++i)
for(unsigned int j=0;j<12;++j)
colorSystem(i,j)+=eq[row][i]*eq[row][j];
}
}
}
}
/* Find the linear system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> qe=colorSystem.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(qe.second(0,0));
for(unsigned int i=1;i<12;++i)
{
if(minE>Math::abs(qe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(qe.second(i,0));
}
}
std::cout<<"Smallest eigenvalue of color system = "<<minE<<std::endl;
/* Create the normalized homography and extend it to a 4x4 matrix: */
Math::Matrix colorMatrix(4,4);
double cmScale=qe.first(11,minEIndex);
for(int i=0;i<2;++i)
for(int j=0;j<4;++j)
colorMatrix(i,j)=qe.first(i*4+j,minEIndex)/cmScale;
for(int j=0;j<4;++j)
colorMatrix(2,j)=j==2?1.0:0.0;
for(int j=0;j<4;++j)
colorMatrix(3,j)=qe.first(2*4+j,minEIndex)/cmScale;
/* Un-normalize the color matrix: */
for(int i=0;i<4;++i)
{
colorMatrix(i,0)/=640.0;
colorMatrix(i,1)/=480.0;
colorMatrix(i,2)/=double(maxDepth);
}
/* Calculate the approximation error: */
double rms=0.0;
unsigned int numPoints=0;
depthPoint(3)=1.0;
for(int y=20;y<480;y+=40)
{
depthPoint(1)=double(y)+0.5;
for(int x=20;x<640;x+=40)
{
depthPoint(0)=double(x)+0.5;
for(int depth=minDepth;depth<=maxDepth;depth+=20)
{
/* Transform the depth point to color image space using the non-linear transformation: */
// int xdisp=x+Math::floor(dispA+dispB*double(depth)+0.5);
// if(xdisp>=0&&xdisp<640)
{
// double colX=double(xdisp)/640.0;
// double colY=double(y)/480.0;
// double colX=colorx[y*640+xdisp]/640.0;
// double colY=(colory[y*640+xdisp]-50.0)/480.0;
double colX=(colorx[y*640+x]+dispA+dispB*double(depth))/640.0;
double colY=(colory[y*640+x]-0.0)/480.0;
/* Transform the depth image point to color space using the color matrix: */
depthPoint(2)=double(depth);
Math::Matrix colorPoint=colorMatrix*depthPoint;
/* Calculate the approximation error: */
double dist=Math::sqr(colorPoint(0)/colorPoint(3)-colX)+Math::sqr(colorPoint(1)/colorPoint(3)-colY);
rms+=dist;
++numPoints;
}
}
}
}
/* Print the RMS: */
std::cout<<"Color matrix approximation RMS = "<<Math::sqrt(rms/double(numPoints))<<std::endl;
/* Make the color matrix compatible with Kinect::Projector's way of doing things: */
// colorMatrix*=depthMatrix;
{
std::cout<<"Optimal homography from world space to color image space:"<<std::endl;
std::streamsize oldPrec=std::cout.precision(8);
std::cout.setf(std::ios::fixed);
for(int i=0;i<4;++i)
{
std::cout<<" ";
for(int j=0;j<4;++j)
std::cout<<' '<<std::setw(11)<<colorMatrix(i,j);
std::cout<<std::endl;
}
std::cout.unsetf(std::ios::fixed);
std::cout.precision(oldPrec);
}
delete[] colorx;
delete[] colory;
/* Convert the depth matrix from mm to cm: */
Math::Matrix scaleMatrix(4,4,1.0);
for(int i=0;i<3;++i)
scaleMatrix(i,i)=0.1;
depthMatrix=scaleMatrix*depthMatrix;
/* Write the depth and color matrices to an intrinsic parameter file: */
std::string calibFileName=KINECT_CONFIG_DIR;
calibFileName.push_back('/');
calibFileName.append(KINECT_CAMERA_INTRINSICPARAMETERSFILENAMEPREFIX);
calibFileName.push_back('-');
calibFileName.append(camera.getSerialNumber());
calibFileName.append(".dat");
if(!Misc::doesPathExist(calibFileName.c_str()))
{
std::cout<<"Writing full intrinsic camera parameters to "<<calibFileName<<std::endl;
IO::FilePtr calibFile=IO::openFile(calibFileName.c_str(),IO::File::WriteOnly);
calibFile->setEndianness(Misc::LittleEndian);
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
calibFile->write<double>(depthMatrix(i,j));
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
calibFile->write<double>(colorMatrix(i,j));
}
else
std::cout<<"Intrinsic camera parameter file "<<calibFileName<<" already exists"<<std::endl;
}
Vec2 Node::convertToWindowSpace(const Vec2& nodePoint) const
{
Vec2 worldPoint(this->convertToWorldSpace(nodePoint));
return _director->convertToUI(worldPoint);
}
CPoint CConvert::GetWorldPointByGridPoint(CPoint gp){
CPoint mapPoint(GetMapPointByGridPoint(gp));
CPoint worldPoint(GetWorldPointByMapPoint(mapPoint));
return worldPoint;
}
