예제 #1
0
static unsigned int
nr_arena_group_update (NRArenaItem *item, NRRectL *area, NRGC *gc, unsigned int state, unsigned int reset)
{
    unsigned int newstate;

    NRArenaGroup *group = NR_ARENA_GROUP (item);

    unsigned int beststate = NR_ARENA_ITEM_STATE_ALL;

    for (NRArenaItem *child = group->children; child != NULL; child = child->next) {
        NRGC cgc(gc);
        cgc.transform = group->child_transform * gc->transform;
        newstate = nr_arena_item_invoke_update (child, area, &cgc, state, reset);
        beststate = beststate & newstate;
    }

    if (beststate & NR_ARENA_ITEM_STATE_BBOX) {
        nr_rect_l_set_empty (&item->bbox);
        for (NRArenaItem *child = group->children; child != NULL; child = child->next) {
            if (child->visible)
                nr_rect_l_union (&item->bbox, &item->bbox, &child->drawbox);
        }
    }

    return beststate;
}
void calibrate(const std::vector<TiePoint>& tiePoints,const int gridSize[2],const double tileSize[2],const unsigned int depthFrameSize[2],const unsigned int colorFrameSize[2])
{
    /* Initialize the depth camera's intrinsic parameter matrix: */
    Math::Matrix depthV(6,6,0.0);

    /* Process all tie points: */
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        /* Enter the tie point's depth homography into the intrinsic parameter matrix: */
        Homography::Matrix hm(1.0);
        hm(0,2)=double(depthFrameSize[0]);
        hm*=tpIt->depthHom.getMatrix();
        Homography::Matrix scale(1.0);
        scale(0,0)=1.0/tileSize[0];
        scale(1,1)=1.0/tileSize[1];
        hm*=scale;
        double row[3][6];
        static const int is[3]= {0,0,1};
        static const int js[3]= {1,0,1};
        for(int r=0; r<3; ++r)
        {
            int i=is[r];
            int j=js[r];
            row[r][0]=hm(0,i)*hm(0,j);
            row[r][1]=hm(0,i)*hm(1,j)+hm(1,i)*hm(0,j);
            row[r][2]=hm(0,i)*hm(2,j)+hm(2,i)*hm(0,j);
            row[r][3]=hm(1,i)*hm(1,j);
            row[r][4]=hm(1,i)*hm(2,j)+hm(2,i)*hm(1,j);
            row[r][5]=hm(2,i)*hm(2,j);
        }
        for(int i=0; i<6; ++i)
            row[1][i]-=row[2][i];
        for(int r=0; r<2; ++r)
        {
            for(unsigned int i=0; i<6; ++i)
                for(unsigned int j=0; j<6; ++j)
                    depthV(i,j)+=row[r][i]*row[r][j];
        }
    }

    /* Find the intrinsic parameter linear system's smallest eigenvalue: */
    std::pair<Math::Matrix,Math::Matrix> depthQe=depthV.jacobiIteration();
    unsigned int minEIndex=0;
    double minE=Math::abs(depthQe.second(0));
    for(unsigned int i=1; i<6; ++i)
    {
        if(minE>Math::abs(depthQe.second(i)))
        {
            minEIndex=i;
            minE=Math::abs(depthQe.second(i));
        }
    }
    std::cout<<"Smallest eigenvalue of v = "<<depthQe.second(minEIndex)<<std::endl;

    /* Calculate the intrinsic parameters: */
    Math::Matrix b=depthQe.first.getColumn(minEIndex);
    std::cout<<b(0)<<", "<<b(1)<<", "<<b(2)<<", "<<b(3)<<", "<<b(4)<<", "<<b(5)<<std::endl;
    double v0=(b(1)*b(2)-b(0)*b(4))/(b(0)*b(3)-Math::sqr(b(1)));
    double lambda=b(5)-(Math::sqr(b(2))+v0*(b(1)*b(2)-b(0)*b(4)))/b(0);
    double alpha=Math::sqrt(lambda/b(0));
    double beta=Math::sqrt(lambda*b(0)/(b(0)*b(3)-Math::sqr(b(1))));
    double gamma=-b(1)*Math::sqr(alpha)*beta/lambda;
    double u0=gamma*v0/beta-b(2)*Math::sqr(alpha)/lambda;

    std::cout<<"Intrinsic camera parameters:"<<std::endl;
    std::cout<<alpha<<" "<<gamma<<" "<<u0<<std::endl;
    std::cout<<0.0<<" "<<beta<<" "<<v0<<std::endl;
    std::cout<<0.0<<" "<<0.0<<" "<<1.0<<std::endl;

    /* Create the intrinsic camera parameter matrix: */
    Math::Matrix a(3,3,1.0);
    a.set(0,0,alpha);
    a.set(0,1,gamma);
    a.set(0,2,u0);
    a.set(1,1,beta);
    a.set(1,2,v0);
    Math::Matrix aInv=a.inverse();

    /* Calculate extrinsic parameters for each tie point to get measurements for the depth formula regression: */
    Math::Matrix depthAta(2,2,0.0);
    Math::Matrix depthAtb(2,1,0.0);
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        /* Convert the tie point's depth homography to a matrix: */
        Homography::Matrix hm(1.0);
        hm(0,2)=double(depthFrameSize[0]);
        hm*=tpIt->depthHom.getMatrix();
        Homography::Matrix scale(1.0);
        scale(0,0)=1.0/tileSize[0];
        scale(1,1)=1.0/tileSize[1];
        hm*=scale;
        Math::Matrix h(3,3);
        for(unsigned int i=0; i<3; ++i)
            for(unsigned int j=0; j<3; ++j)
                h(i,j)=hm(i,j);

        /* Calculate the extrinsic parameters: */
        double lambda=0.5/(aInv*h.getColumn(0)).mag()+0.5/(aInv*h.getColumn(1)).mag();
        Math::Matrix r1=lambda*aInv*h.getColumn(0);
        Math::Matrix r2=lambda*aInv*h.getColumn(1);
        Math::Matrix r3(3,1);
        for(unsigned int i=0; i<3; ++i)
            r3.set(i,r1((i+1)%3)*r2((i+2)%3)-r1((i+2)%3)*r2((i+1)%3)); // 'Tis a cross product, in case you're wondering
        Math::Matrix t=lambda*aInv*h.getColumn(2);

        /* Create the extrinsic parameter matrix: */
        Math::Matrix rt(3,4);
        rt.setColumn(0,r1);
        rt.setColumn(1,r2);
        rt.setColumn(2,r3);
        rt.setColumn(3,t);

        Math::Matrix wgc(4,1);
        wgc(0)=tileSize[0]*double(gridSize[0])*0.5;
        wgc(1)=tileSize[1]*double(gridSize[1])*0.5;
        wgc(2)=0.0;
        wgc(3)=1.0;
        Math::Matrix cgc=rt*wgc;
        if(cgc(2)<0.0)
        {
            /* Flip the extrinsic matrix to move the grid to positive z: */
            Math::Matrix flip(3,3,-1.0);
            rt=flip*rt;
            cgc=rt*wgc;
        }
        std::cout<<"Grid center: "<<cgc(0)<<", "<<cgc(1)<<", "<<cgc(2)<<std::endl;

        /* Transform all world grid points with the extrinsic matrix to get their camera-space z values: */
        for(int y=0; y<gridSize[1]; ++y)
            for(int x=0; x<gridSize[0]; ++x)
            {
                /* Create the world point: */
                Math::Matrix wp(4,1);
                wp(0)=tileSize[0]*double(x);
                wp(1)=tileSize[1]*double(y);
                wp(2)=0.0;
                wp(3)=1.0;

                /* Transform the world point to camera space: */
                Math::Matrix cp=rt*wp;
                double dist=cp(2);

                /* Get the depth frame value from the grid's plane in depth image space: */
                Point dip=tpIt->depthHom.transform(Point(x,y));
                const Plane::Vector& n=tpIt->gridPlane.getNormal();
                double o=tpIt->gridPlane.getOffset();
                double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];

                /* Enter the depth / z pair into the depth formula accumulator: */
                depthAta(0,0)+=1.0;
                depthAta(0,1)+=-dist;
                depthAta(1,0)+=-dist;
                depthAta(1,1)+=dist*dist;
                depthAtb(0)+=-dist*depth;
                depthAtb(1)+=dist*dist*depth;
            }
    }

    /* Solve the depth formula least-squares system: */
    Math::Matrix depthX=depthAtb.divideFullPivot(depthAta);
    std::cout<<"Depth conversion formula: dist = "<<depthX(0)<<" / ("<<depthX(1)<<" - depth)"<<std::endl;

    /* Calculate the full depth unprojection matrix: */
    Math::Matrix depthProj(4,4,0.0);
    depthProj(0,0)=1.0/alpha;
    depthProj(0,1)=-gamma/(alpha*beta);
    depthProj(0,3)=-u0/alpha+v0*gamma/(alpha*beta);
    depthProj(1,1)=1.0/beta;
    depthProj(1,3)=-v0/beta;
    depthProj(2,3)=-1.0;
    depthProj(3,2)=-1.0/depthX(0);
    depthProj(3,3)=depthX(1)/depthX(0);

    Math::Matrix colorAta(12,12,0.0);
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        for(int y=0; y<gridSize[1]; ++y)
            for(int x=0; x<gridSize[0]; ++x)
            {
                /* Enter the tie point into the color calibration matrix linear system: */
                Point dip=tpIt->depthHom.transform(Point(x,y));
                const Plane::Vector& n=tpIt->gridPlane.getNormal();
                double o=tpIt->gridPlane.getOffset();
                double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
                Math::Matrix dwp(4,1);
                dwp(0)=dip[0]+double(depthFrameSize[0]);
                dwp(1)=dip[1];
                dwp(2)=depth;
                dwp(3)=1.0;
                dwp=depthProj*dwp;
                for(int i=0; i<3; ++i)
                    dwp(i)/=dwp(3);
                Point cip=tpIt->colorHom.transform(Point(x,y));
                cip[0]/=double(colorFrameSize[0]);
                cip[1]/=double(colorFrameSize[1]);

                double eq[2][12];
                eq[0][0]=dwp(0);
                eq[0][1]=dwp(1);
                eq[0][2]=dwp(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]=-cip[0]*dwp(0);
                eq[0][9]=-cip[0]*dwp(1);
                eq[0][10]=-cip[0]*dwp(2);
                eq[0][11]=-cip[0];

                eq[1][0]=0.0;
                eq[1][1]=0.0;
                eq[1][2]=0.0;
                eq[1][3]=0.0;
                eq[1][4]=dwp(0);
                eq[1][5]=dwp(1);
                eq[1][6]=dwp(2);
                eq[1][7]=1.0;
                eq[1][8]=-cip[1]*dwp(0);
                eq[1][9]=-cip[1]*dwp(1);
                eq[1][10]=-cip[1]*dwp(2);
                eq[1][11]=-cip[1];

                for(int row=0; row<2; ++row)
                {
                    for(unsigned int i=0; i<12; ++i)
                        for(unsigned int j=0; j<12; ++j)
                            colorAta(i,j)+=eq[row][i]*eq[row][j];
                }
            }
    }

    /* Find the color calibration system's smallest eigenvalue: */
    std::pair<Math::Matrix,Math::Matrix> colorQe=colorAta.jacobiIteration();
    minEIndex=0;
    minE=Math::abs(colorQe.second(0,0));
    for(unsigned int i=1; i<12; ++i)
    {
        if(minE>Math::abs(colorQe.second(i,0)))
        {
            minEIndex=i;
            minE=Math::abs(colorQe.second(i,0));
        }
    }

    /* Create the normalized color homography: */
    Math::Matrix colorHom(3,4);
    double scale=colorQe.first(11,minEIndex);
    for(int i=0; i<3; ++i)
        for(int j=0; j<4; ++j)
            colorHom(i,j)=colorQe.first(i*4+j,minEIndex)/scale;

    /* Create the full color unprojection matrix by extending the homography: */
    Math::Matrix colorProj(4,4);
    for(unsigned int i=0; i<2; ++i)
        for(unsigned int j=0; j<4; ++j)
            colorProj(i,j)=colorHom(i,j);
    for(unsigned int j=0; j<4; ++j)
        colorProj(2,j)=j==2?1.0:0.0;
    for(unsigned int j=0; j<4; ++j)
        colorProj(3,j)=colorHom(2,j);

    /* Modify the color unprojection matrix by the depth projection matrix: */
    colorProj*=depthProj;

    /* Write the calibration file: */
    IO::FilePtr calibFile(IO::openFile("CameraCalibrationMatrices.dat",IO::File::WriteOnly));
    calibFile->setEndianness(Misc::LittleEndian);
    for(int i=0; i<4; ++i)
        for(int j=0; j<4; ++j)
            calibFile->write<double>(depthProj(i,j));
    for(int i=0; i<4; ++i)
        for(int j=0; j<4; ++j)
            calibFile->write<double>(colorProj(i,j));
}