osg::Matrix vpb::MyDataSet::getImageSection(vips::VImage &in,const osg::Vec2 minT, const osg::Vec2 maxT,int origX,int origY,osg::Vec4 &texsize,const osg::Matrix &toTex,osg::ref_ptr<osg::Image> &image,osg::Vec4 &ratio,int level){
double downsampleFactor=pow(2,level);
double downsampleRatio=1.0/downsampleFactor;
// std::cout<< minT << " " << maxT<<std::endl;
// printf("%f %f\n",downsampleRatio,downsampleRatio);
int x=(int)std::max((int)floor(minT.x()),0);
int y=(int)std::max((int)floor(minT.y()),0);
int xMax=(int)std::min((int)ceil(maxT.x()),origX);
int yMax=(int)std::min((int)ceil(maxT.y()),origY);
int xRange=(xMax-x);
int yRange=(yMax-y);
//printf("X:%d -- %d Y:%d -- %d ",x,xMax,y,yMax);
//Need bias of 1.0 or will round down
double maxSide=std::max(osg::Image::computeNearestPowerOfTwo(xRange,1.0),osg::Image::computeNearestPowerOfTwo(yRange,1.0));
osg::Vec2 subSize=osg::Vec2(maxSide,maxSide);
if(downsampleRatio*maxSide < 1.0){
printf("Clipping %f %f to",downsampleRatio,downsampleFactor);
downsampleRatio=1.0/maxSide;
downsampleFactor=maxSide;
printf("%f %f\n",downsampleRatio,downsampleFactor);
}
texsize[0]=origX;
texsize[1]=origY;
texsize[2]=subSize.x();
texsize[3]=subSize.y();
osg::Vec2 downsampleSize(subSize.x(),subSize.y());
downsampleSize.x()*=downsampleRatio;
downsampleSize.y()*=downsampleRatio;
// printf("Range %d %d\n",xRange,yRange);
// printf("%f %f %f %f\n",subSize.x(),subSize.y(),downsampleSize.x(),downsampleSize.y());
image = new osg::Image;
image->allocateImage(downsampleSize.x(),downsampleSize.y(), 1, GL_RGB,GL_UNSIGNED_BYTE);
if(image->data() == 0 ){
fprintf(stderr,"Failed to allocate\n");
exit(-1);
}
{
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_imageMutex);
vips::VImage osgImage(image->data(),downsampleSize.x(),downsampleSize.y(),3,vips::VImage::FMTUCHAR);
in.extract_area(x,y,xRange,yRange).embed(1,0,0,subSize.x(),subSize.y()).shrink(downsampleFactor,downsampleFactor).write(osgImage);
}
ratio=osg::Vec4(x,y,subSize.x(),subSize.y());
//osg::Vec2 f(xRange-subSize.x(),yRange-subSize.y());
//std::cout << f<<std::endl;
return toTex;
}
MatrixXf computeDepthImage( osg::Camera* camera, osg::ref_ptr<osg::Image> depthbufferimg ) {
cout << "computing depth image" << endl;
// from https://svn.lcsr.jhu.edu/cisst/trunk/saw/components/sawOpenSceneGraph/code/osaOSGCamera.cpp
// This is used by glutUnProject
const osg::Viewport* viewport = camera->getViewport();
size_t width = viewport->width();
size_t height = viewport->height();
// Create a 3xN range data destination matrix.
// [ x1 ... xN ]
// [ y1 ... yN ]
// [ z1 ... zN ]
// VCT_COL_MAJOR is used because we will write in the following order
// x1, y1, z1, x2, y2, z2, ..., xN, yN, zZ
MatrixXf depthimage( width, height);
float* Z = depthimage.data();
float* z = (float*)depthbufferimg->data();
// get the intrinsic parameters of the camera
double fovy, aspectRatio, Zn, Zf;
camera->getProjectionMatrixAsPerspective( fovy, aspectRatio, Zn, Zf );
size_t i=0;
// convert zbuffer values [0,1] to depth values + flip the image vertically
for( int r=height-1; 0<=r; r-- ){
for( int c=0; c<width; c++ ){
// forgot where I took this equation
Z[ i++ ] = Zn*Zf / (Zf - z[ r*width + c ]*(Zf-Zn));
}
}
return depthimage;
}
GeometryTexturePatch16::GeometryTexturePatch16( int x , int y , int sizeC , int scaleC , osg::ref_ptr<osg::Image> image , double dAdd , double dScale )
{
//сдвиг и масштабирование
m_dAdd = dAdd;
m_dScale = dScale;
//создать объект для хранения в нем геометрии
m_patchGeom = new osg::Geometry;
//создать массив вершин
m_patchGeom->setVertexArray( CreateVertexArray( x , y , sizeC , scaleC ).get() );
//создать массив текстурных координат
m_patchGeom->setTexCoordArray( 0, CreateTexCoordArray( x , y , sizeC , scaleC ,
image->data() ).get() );
// Create an array for the single normal.
osg::ref_ptr< osg::Vec3Array > n = new osg::Vec3Array;
n->push_back( osg::Vec3( 0, -1, 0 ) );
m_patchGeom->setNormalArray( n.get() );
m_patchGeom->setNormalBinding( osg::Geometry::BIND_OVERALL );
std::vector< unsigned int > m_vIndex;
//заполнить вектор индексами
FillIndexVector( m_vIndex , sizeC );
m_patchGeom->addPrimitiveSet( new osg::DrawElementsUInt(
osg::PrimitiveSet::TRIANGLE_STRIP, m_vIndex.size() , &m_vIndex[ 0 ] ) );
}
ColorCloudPtr computePointCloud( osg::Camera* camera, osg::ref_ptr<osg::Image> depthbufferimg, osg::ref_ptr<osg::Image> colorbufferimg) {
MatrixXf X,Y,Z;
computeRangeData(camera, depthbufferimg, X, Y, Z);
int nRows = X.rows();
int nCols = X.cols();
ColorCloudPtr out(new ColorCloud(nCols, nRows));
for (int row=0; row < nRows; row++) {
for (int col=0; col < nCols; col++) {
ColorPoint& pt = out->at(col, row);
pt.x = X(row,col);
pt.y = Y(row,col);
pt.z = Z(row,col);
pt.r = *(colorbufferimg->data(col,row));
pt.g = *(colorbufferimg->data(col,row));
pt.b = *(colorbufferimg->data(col,row));
}
}
return out;
}
void computeRangeData( osg::Camera* camera , osg::ref_ptr<osg::Image> depthbufferimg, MatrixXf& Ximg, MatrixXf& Yimg, MatrixXf& Zimg) {
// from https://svn.lcsr.jhu.edu/cisst/trunk/saw/components/sawOpenSceneGraph/code/osaOSGCamera.cpp
// This is used by glutUnProject
const osg::Viewport* viewport = camera->getViewport();
size_t width = viewport->width();
size_t height = viewport->height();
GLint view[4];
view[0] = (int)viewport->x();
view[1] = (int)viewport->y();
view[2] = width;
view[3] = height;
// Create a 3xN range data destination matrix.
// [ x1 ... xN ]
// [ y1 ... yN ]
// [ z1 ... zN ]
// VCT_COL_MAJOR is used because we will write in the following order
// x1, y1, z1, x2, y2, z2, ..., xN, yN, zZ
Ximg.resize(height,width);
Yimg.resize(height,width);
Zimg.resize(height,width);
// get the intrinsic parameters of the camera
double fovy, aspectRatio, Zn, Zf;
camera->getProjectionMatrixAsPerspective( fovy, aspectRatio, Zn, Zf );
osg::Matrixd modelm = camera->getViewMatrix();
osg::Matrixd projm = camera->getProjectionMatrix();
//for( int y=0; y<height; y++ ){
for( int y=height-1; 0<=y; y-- ){
for( size_t x=0; x<width; x++ ){
GLdouble X, Y, Z;
float* d = (float*)depthbufferimg->data( x, y );
gluUnProject( x, y, *d, modelm.ptr(), projm.ptr(), view, &X, &Y, &Z );
//gluUnProject( x, y, *d, &model[0][0], &proj[0][0], view, &X, &Y, &Z );
// rangedata is 4xN column major
Ximg(y,x) = X;
Yimg(y,x) = Y;
Zimg(y,x) = Z;
}
}
}
