    (*********************************************************************
     *  cvAccess                                                         *
     *                                                                   *
     *   by Vladimir Vasilyev                                            *
     *   home page : http://www.nextnow.com                              *
     *   email     : Vladimir@nextnow.com                                *
     *               W-develop@mtu-net.ru                                *
     *                                                                   *
     *                                                                   *
     *                                                                   *
     *  The contents of this file are used with permission, subject to   *
     *  the Mozilla Public License Version 1.1 (the "License"); you may  *
     *  not use this file except in compliance with the License. You may *
     *  obtain a copy of the License at                                  *
     *  http://www.mozilla.org/MPL/MPL-1.1.html                          *
     *                                                                   *
     *  Software distributed under the License is distributed on an      *
     *  "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or   *
     *  implied. See the License for the specific language governing     *
     *  rights and limitations under the License.                        *
     *                                                                   *
     *********************************************************************)

unit cvAccess;

interface
 Uses
  Windows, Messages, SysUtils, Classes, Graphics, Controls, Forms, Dialogs, StdCtrls,
  G32_WConvolution, OpenCV;


Procedure FindExtrinsicCameraParams(var InArr : T2DPoint3fArr; Out RArr, TArr, MArr, InvMArr : T2DPoint1fArr );overload;

Procedure cvMInit( M  : PCvMat; val : single );
Procedure cvmPrint( M : PCvMat; Arr : T2DPoint1fArr );
Function  GetAngles( RotArr : T2DPoint1fArr; Var a, b, g : single) : Boolean;
Procedure FindObjectPoints( InArr : T2DPoint3fArr; OutArr : T2DPoint3fArr; M : PCvArr );


Function FindChessBoardCorner( PicFileName: string; RowCount,ColCount : Integer;
                               Out CornerArr : T2DPoint2fArr ) : boolean;

Function FindExtrinsicCameraParams( PicFileName: string; RowCount,ColCount : Integer;
                                    squarewidth : single;
                                    xDist, yDist,f : Double;
                                    Out RArr, TArr, MArr, InvMArr : T2DPoint1fArr;
                                    Out CornerArr : T2DPoint2fArr ) : boolean;overload;
                                          
Function CalibrateCamera( InArr : TPoint2fArr;
                          NumImages, RowCount,ColCount : Integer;
                          squarewidth : single;
                          xDist, yDist,f : Double;
                          ImageWidth,ImageHeight: Integer;
                          Out DistrArr, CameraMArr : T2DPoint1fArr ) : boolean;

implementation
Uses Math,IPL,Pathes;


Procedure SetPoint3D32f( p3f : PCvPoint3D32f; i : integer; val : TCvPoint3D32f );
begin
 inc(p3f,i);
 p3f^:=val;
end;

Procedure ReSortCorners( imagePoints : PCvPoint2D32f; numPoints, RowCount, ColCount : Integer );
Var
 TempCorners    : PCvPoint2D32f;
 p1,p2          : TCvPoint2D32f;
 i,j,pos1,pos2  : Integer;
begin
 p1:=TCvPoint2D32fArr(imagePoints)[0];
 p2:=TCvPoint2D32fArr(imagePoints)[numPoints-1];

 GetMem( TempCorners, numPoints*SizeOf(TCvPoint2D32f) );
 try
 
    if (p1.x<p2.x) then                               // 1 2
       if (p1.y<p2.y) then begin                      // 3 4
          Exit // 1-4 - правильный порядок: 
       end  
       else   // 3-2
          for i:=0 to RowCount-1 do
              for j:=0 to ColCount-1 do begin
                  pos1:=i*ColCount+j;
                  pos2:=(RowCount-1-i)*ColCount +j; 
                  TCvPoint2D32fArr(TempCorners)[pos2]:=TCvPoint2D32fArr(imagePoints)[pos1];
              end;
 
    if p1.x>p2.x then 
       if p1.y<p2.y then begin // 2-3
          for i:=0 to RowCount-1 do
              for j:=0 to ColCount-1 do begin
                  pos1:=i*ColCount+j;
                  pos2:=i*ColCount +(ColCount-1-j); 
                  TCvPoint2D32fArr(TempCorners)[pos2]:=TCvPoint2D32fArr(imagePoints)[pos1];
              end;
       end
       else begin // 4-1
          for i:=0 to RowCount-1 do
              for j:=0 to ColCount-1 do begin
                  pos1:=i*ColCount+j;
                  pos2:=(RowCount-1-i)*ColCount +(ColCount-1-j); 
                  TCvPoint2D32fArr(TempCorners)[pos2]:=TCvPoint2D32fArr(imagePoints)[pos1];
              end;
       end;

    Move(TempCorners^, imagePoints^, numPoints*SizeOf(TCvPoint2D32f));
    
 finally        
    FreeMem(TempCorners);
 end;
   
end;



Function CalibrateCamera( InArr : TPoint2fArr;
                          NumImages, RowCount,ColCount : Integer;
                          squarewidth : single;
                          xDist, yDist,f : Double;
                          ImageWidth,ImageHeight: Integer;
                          Out DistrArr, CameraMArr : T2DPoint1fArr ) : boolean;
Var
 i,j,k,n,
 numPoints, pos    : integer;
 fx,fy,cx,cy       : single;
 imagePoints       : PCvPoint2D32f;
 objectPoints      : PCvPoint3D32f;
 distortion,
 transV            : PCvVect32f;
 cameraM,         
 rotM              : PCvMatr32f;
 imageSize         : TCvSize;
 p3f               : TCvPoint3D32f;
 pNumPointsArr     : PInteger;
 useIntrinsicGuess : Integer; 
begin
 Result:=false;
try
 imageSize:=CvSize_(ImageWidth,ImageHeight);
 
 cx:=ImageWidth/2;        {pix}// = 640
 cy:=ImageHeight/2;       {pix}// = 480

 fx:=f/xDist;         {pix}
 fy:=f/yDist;         {pix}

 numPoints:=RowCount*ColCount;

 //массив значений количества точек в каждом отдельном имадже - здесь везде одинаков 
 GetMem(pNumPointsArr, NumImages*SizeOf(Integer));
 for i:=0 to NumImages-1 do
     TIntegerArr(pNumPointsArr)[i]:=numPoints;
 
 GetMem( imagePoints,  NumImages*numPoints*SizeOf(TCvPoint2D32f) );
 Assert( Length(InArr)=NumImages*numPoints);
 for n:=0 to High(InArr) do begin
     TCvPoint2D32fArr(imagePoints)[n]:=TCvPoint2D32f(InArr[n]);
 end;

 GetMem( objectPoints, NumImages*numPoints*SizeOf(TCvPoint3D32f));
 for n:=0 to NumImages-1 do
     for i:=0 to RowCount-1 do
         for j:=0 to ColCount-1 do begin
             p3f.x:=j*squarewidth;
             p3f.y:=i*squarewidth;
             p3f.z:=0;
             TCvPoint3D32fArr(objectpoints)[n*numPoints + i*ColCount+j]:=p3f;
         end;

 GetMem(distortion,4*SizeOf(Single));
 for i:=0 to 3 do
     TCvVect32fArr(distortion)[i]:=0;
     
 GetMem(cameraM, 3*3*SizeOf(Single));     
 for i:=0 to 9 do
     TCvVect32fArr(cameraM)[i]:=0;
 TCvVect32fArr(cameraM)[0]:=fx;
 TCvVect32fArr(cameraM)[4]:=fy;
 TCvVect32fArr(cameraM)[2]:=cx;
 TCvVect32fArr(cameraM)[5]:=cy;
 TCvVect32fArr(cameraM)[8]:=1;

 GetMem(transV,  3*SizeOf(Single)*NumImages);
 GetMem(rotM,    3*3*SizeOf(Single)*NumImages);
 
 //useIntrinsicGuess:=0;// не использовать
 useIntrinsicGuess:=1;// использовать
 
 cvCalibrateCamera( numimages, 
                    pNumPointsArr, 
                    imageSize, 
                    imagePoints,  // UV 
                    objectpoints, // XYZ 
                    distortion, 
                    cameraM, 
                    transV, 
                    rotM, 
                    useIntrinsicGuess
                    ); 

 SetLength(DistrArr,1,4);
 for i:=0 to 3 do
     DistrArr[0,i]:=TCvVect32fArr(distortion)[i];
    

 SetLength(CameraMArr,3,3);
 for i:=0 to 2 do
     for j:=0 to 2 do begin
         pos:=i*3+j;
         CameraMArr[i,j]:=TCvVect32fArr(cameraM)[pos];
     end;
     
 Result:=true;
 
finally
 FreeMem(rotM);  
 FreeMem(transV);
 FreeMem(cameraM);
 FreeMem(distortion);
 FreeMem(objectPoints);
 FreeMem(imagePoints);
 FreeMem(pNumPointsArr);
end;

end;


Function FindChessBoardCorner( PicFileName: string; RowCount,ColCount : Integer;
                               Out CornerArr : T2DPoint2fArr ) : boolean;
Var
 i,j,numPoints    : integer;
 pos              : integer;
 calibimage,
 greyimg,img0     : PIplImage;
 imagePoints      : PCvPoint2D32f;
 imageSize        : TCvSize;
 storage          : PCvMemStorage;
 TermCriteria     : TCvTermCriteria;
 Res              : Integer;
 p2f              : TCvPoint2D32f;
begin
 Result:=false;
try

   calibimage:=cvLoadImage(PChar(PicFileName));
   imageSize:=CvSize_(calibimage.Width,calibimage.Height);
   numPoints:=RowCount*ColCount;
 
   GetMem( imagePoints,  numPoints*SizeOf(TCvPoint2D32f) );

   greyimg:=nil;
   greyimg:=cvCreateImageHeader(imageSize,IPL_DEPTH_8U,1);
   cvCreateData(greyimg);
   cvCvtColor(calibimage, greyimg, CV_RGB2GRAY);
   img0:=cvCloneImage(greyimg);
   storage:=cvCreateMemStorage(0);

   Res:=cvFindChessBoardCornerGuesses(greyimg, img0, storage, cvSize_(ColCount,RowCount), imagePoints, @numPoints);
   if Res=0 then Exit;

   TermCriteria.type_:=CV_TERMCRIT_ITER or CV_TERMCRIT_EPS;
   TermCriteria.maxIter:=10;
   TermCriteria.epsilon:=0.1;
 
   cvFindCornerSubPix( greyimg,
                       imagePoints,
                       numPoints, 
                       cvSize_(6,6), 
                       cvSize_(-1,-1),
                       TermCriteria );

   ReSortCorners( imagePoints, numPoints, RowCount, ColCount );
                     
   SetLength(CornerArr,RowCount, ColCount); 
   for i:=0 to RowCount-1 do
       for j:=0 to ColCount-1 do begin
           pos:=i*ColCount+j;
           p2f:=TCvPoint2D32fArr(imagePoints)[pos];
           CornerArr[i,j]:=TPoint2f(p2f);
       end;

     
   Result:=true;
 finally
   FreeMem(imagePoints);

   cvReleaseImage(calibimage);
   cvReleaseImage(greyimg);
   cvReleaseImage(img0);
   cvReleaseMemStorage(storage);
 end;  
end;

Function FindExtrinsicCameraParams( PicFileName: string; RowCount,ColCount : Integer;
                                    squarewidth : single;
                                    xDist, yDist,f : Double;
                                    Out RArr, TArr, MArr, InvMArr : T2DPoint1fArr;
                                    Out CornerArr : T2DPoint2fArr ) : boolean;
Var
 i,j,numPoints    : integer;
 pos              : integer;
 fx,fy,cx,cy      : single;

 calibimage,
 greyimg,img0     : PIplImage;

 imagePoints      : PCvPoint2D32f;
 objectPoints     : PCvPoint3D32f;
 focalLength,
 distortion,
 rotVect,
 transVect        : PCvVect32f;
 jaco,
 rotation,
 rotationvect,
 A, M, IC,InvM    : TCvMat;
 imageSize        : TCvSize;
 principalPoint   : TCvPoint2D32f;
 p3f              : TCvPoint3D32f;
 p2f              : TCvPoint2D32f;

 storage          : PCvMemStorage;
 TermCriteria     : TCvTermCriteria;
 Res              : Integer;
begin
 Result:=false;
try

 calibimage:=cvLoadImage(PChar(PicFileName));
 imageSize:=CvSize_(calibimage.Width,calibimage.Height);

 cx:=calibimage.Width/2;        {pix}
 cy:=calibimage.Height/2;       {pix}
 fx:=f/xDist;                   {pix}
 fy:=f/yDist;                   {pix}

 numPoints:=RowCount*ColCount;
 
 GetMem( imagePoints,  numPoints*SizeOf(TCvPoint2D32f) );
 GetMem( objectPoints, numPoints*SizeOf(TCvPoint3D32f));

 for i:=0 to RowCount-1 do
     for j:=0 to ColCount-1 do begin
         p3f.x:=j*squarewidth;
         p3f.y:=i*squarewidth;
         p3f.z:=0;
         TCvPoint3D32fArr(objectpoints)[i*ColCount+j]:=p3f;
         //SetPoint3D32f(objectpoints,i*Col+j,Point3D);
     end;
 
 GetMem(focalLength, 2*SizeOf(Single));
 TCvVect32fArr(focalLength)[0]:=fx;
 TCvVect32fArr(focalLength)[1]:=fy;

 principalPoint.x:=cx;
 principalPoint.y:=cy;

 GetMem(distortion,4*SizeOf(Single));
 for i:=0 to 3 do
     TCvVect32fArr(distortion)[i]:=0;

 GetMem(rotVect,   3*SizeOf(Single));
 GetMem(transVect, 3*SizeOf(Single));
 
 greyimg:=nil;
 greyimg:=cvCreateImageHeader(imageSize,IPL_DEPTH_8U,1);
 cvCreateData(greyimg);
 cvCvtColor(calibimage, greyimg, CV_RGB2GRAY);
 img0:=cvCloneImage(greyimg);
 storage:=cvCreateMemStorage(0);

 Res:=cvFindChessBoardCornerGuesses(greyimg, img0, storage, cvSize_(ColCount,RowCount), imagePoints, @numPoints);
 if Res=0 then Exit;

 TermCriteria.type_:=CV_TERMCRIT_ITER or CV_TERMCRIT_EPS;
 TermCriteria.maxIter:=10;
 TermCriteria.epsilon:=0.1;
 
 cvFindCornerSubPix( greyimg,
                     imagePoints,
                     numPoints, 
                     cvSize_(6,6), 
                     cvSize_(-1,-1),
                     TermCriteria );

 ReSortCorners( imagePoints, numPoints, RowCount, ColCount );
                     
 SetLength(CornerArr,RowCount, ColCount); 
 for i:=0 to RowCount-1 do
     for j:=0 to ColCount-1 do begin
         pos:=i*ColCount+j;
         p2f:=TCvPoint2D32fArr(imagePoints)[pos];
         CornerArr[i,j]:=TPoint2f(p2f);
     end;
 
 cvFindExtrinsicCameraParams(  numPoints,
                               imageSize,
                               imagePoints,
                               objectPoints,
                               focalLength,
                               principalPoint,
                               distortion,
                               rotVect,
                               transVect  );
                              
 SetLength(TArr,1,3);
 for i:=0 to 2 do
     TArr[0,i]:=TCvVect32fArr(transVect)[i];
                             
 jaco:= cvMat_(3,9,CV_32F,nil);
 cvCreateData(@jaco);

 rotation:= cvMat_(3,3,CV_32F,nil);
 cvCreateData(@rotation);

 rotationvect:= cvMat_(1,3,CV_32F,nil);
 cvCreateData(@rotationvect);
 for i:=0 to 2 do
     cvmSet(@rotationvect,0,i,TCvVect32fArr(rotVect)[i]);

 cvRodrigues(@rotation,@rotationvect,@jaco,CV_RODRIGUES_V2M);

 SetLength(RArr,3,3);
 cvmPrint(@rotation,RArr);

 A:=cvMat_(4,4,CV_32F,nil);
 cvCreateData(@A);
 cvMInit(@A,0) ;
 for i:=0 to 2 do
     for j:=0 to 2 do
         cvmSet(@A,i,j,cvmGet(@rotation,i,j));
 for i:=0 to 2 do
     cvmSet(@A,i,3,TCvVect32fArr(transVect)[i]);
 cvmSet(@A,3,3,1);
 
 M := cvMat_(3,4,CV_32F,nil);
 cvCreateData(@M);

 IC := cvMat_(3,4,CV_32F,nil);
 cvCreateData(@IC);
 cvMInit(@IC,0) ;
 cvmSet(@IC,0,0,f );
 cvmSet(@IC,0,2,cx);
 cvmSet(@IC,1,1,f );
 cvmSet(@IC,1,2,cy);
 cvmSet(@IC,2,2,1 );

 cvMatMul(@IC,@A,@M);

 SetLength(MArr,3,4);
 cvmPrint(@M,MArr);

 InvM := cvMat_(4,3,CV_32F,nil);
 cvCreateData(@InvM);

 cvPseudoInverse(@M,@InvM);

 SetLength(InvMArr,4,3);
 cvmPrint(@InvM,InvMArr);

 Result:=true;
 finally
   FreeMem(objectPoints);
   FreeMem(imagePoints);
   FreeMem(focalLength);
   FreeMem(distortion);
   FreeMem(rotVect);
   FreeMem(transVect);

   cvReleaseImage(calibimage);
   cvReleaseImage(greyimg);
   cvReleaseImage(img0);
   cvReleaseMemStorage(storage);

   if Result then begin
      cvReleaseData(@jaco);
      cvReleaseData(@rotation);
      cvReleaseData(@rotationvect);
      cvReleaseData(@A);
      cvReleaseData(@M);
      cvReleaseData(@IC);
      cvReleaseData(@InvM);
   end;
 end;  
end;




Procedure FindExtrinsicCameraParams(var InArr : T2DPoint3fArr; Out RArr, TArr, MArr, InvMArr : T2DPoint1fArr );
Var
 imagePoints      : PCvPoint2D32f;
 objectPoints     : PCvPoint3D32f;
 focalLength,
 distortion,
 rotVect,
 transVect        : PCvVect32f;

 i,j,numPoints    : integer;
 dist             : double;
 f,cx,cy          : single;
 jaco,
 rotation,
 rotationvect,
 A, M, IC,InvM    : TCvMat;
 imageSize        : TCvSize;
 principalPoint   : TCvPoint2D32f;
 Point3D          : TCvPoint3D32f;
 squarewidth      : single;
 pos              : integer;
 OutArr           : T2DPoint3fArr;

 
 X,xx   : TCvMat;
 p3f    : TPoint3f;
 p4f    : TPoint4f;
 w      : Single;

 RowCount,ColCount  : Integer;
 
begin

 imageSize:=CvSize_(1280,960);

 dist:=4.65e-3;     {mm}
 cx:=1280/2;        {pix}
 cy:=960/2;         {pix}
 f:=12.5;           {mm}
 f:=f/dist;         {pix}
 squarewidth:=27.4; {mm}
 RowCount:=Length(InArr);//8
 ColCount:=Length(InArr[0]);//6
 numPoints:=RowCount*ColCount;
 
 GetMem(imagePoints, numPoints*SizeOf(TCvPoint2D32f) );
 if (imagePoints = nil) then beep;

 GetMem( objectPoints, numPoints*SizeOf(TCvPoint3D32f));
 if (objectPoints = nil) then beep;
 
 for i:=0 to RowCount-1 do
  for j:=0 to ColCount-1 do
     begin
         pos:=i*ColCount+j;
         //TCVPoint2D32fArr(imagePoints)[pos]:=TCVPoint2D32f(InArr[i,j]);
         TCVPoint2D32fArr(imagePoints)[pos].x:=InArr[i,j].x;
         TCVPoint2D32fArr(imagePoints)[pos].y:=InArr[i,j].y;

         Point3D.x:=j*squarewidth;
         Point3D.y:=i*squarewidth;
         Point3D.z:=0;
         TCVPoint3D32fArr(objectpoints)[pos]:=Point3D;
     end;

 GetMem(focalLength, 2*SizeOf(Single));
 if (focalLength = nil) then  beep;
 for i:=0 to 1 do
     TCvVect32fArr(focalLength)[i]:=f;

 principalPoint.x:=cx;
 principalPoint.y:=cy;

 GetMem(distortion,4*SizeOf(Single));
 if (distortion = nil) then beep;
 for i:=0 to 3 do
     TCvVect32fArr(distortion)[i]:=0;

 GetMem(rotVect,   3*SizeOf(Single));
 GetMem(transVect, 3*SizeOf(Single));

 cvFindExtrinsicCameraParams( numPoints,
                              imageSize,
                              imagePoints,
                              objectPoints,
                              focalLength,
                              principalPoint,
                              distortion,
                              rotVect, transVect );

 SetLength(TArr,1,3);
 for i:=0 to 2 do
     TArr[0,i]:=TCvVect32fArr(transVect)[i];

 jaco:= cvMat_(3,9,CV_32F,nil);
 cvCreateData(@jaco);

 rotation:= cvMat_(3,3,CV_32F,nil);
 cvCreateData(@rotation);

 rotationvect:= cvMat_(1,3,CV_32F,nil);
 cvCreateData(@rotationvect);
 cvmSet(@rotationvect,0,0,TCvVect32fArr(rotVect)[0]);
 cvmSet(@rotationvect,0,1,TCvVect32fArr(rotVect)[1]);
 cvmSet(@rotationvect,0,2,TCvVect32fArr(rotVect)[2]);

 cvRodrigues(@rotation,@rotationvect,@jaco,CV_RODRIGUES_V2M);

 SetLength(RArr,3,3);
 cvmPrint(@rotation,RArr);

 A:=cvMat_(4,4,CV_32F,nil);
 cvCreateData(@A);
 cvMInit(@A,0) ;

 cvmSet(@A,0,0,cvmGet(@rotation,0,0));
 cvmSet(@A,0,1,cvmGet(@rotation,0,1));
 cvmSet(@A,0,2,cvmGet(@rotation,0,2));
 cvmSet(@A,1,0,cvmGet(@rotation,1,0));
 cvmSet(@A,1,1,cvmGet(@rotation,1,1));
 cvmSet(@A,1,2,cvmGet(@rotation,1,2));
 cvmSet(@A,2,0,cvmGet(@rotation,2,0));
 cvmSet(@A,2,1,cvmGet(@rotation,2,1));
 cvmSet(@A,2,2,cvmGet(@rotation,2,2));
 cvmSet(@A,0,3,TCvVect32fArr(transVect)[0]);
 cvmSet(@A,1,3,TCvVect32fArr(transVect)[1]);
 cvmSet(@A,2,3,TCvVect32fArr(transVect)[2]);
 cvmSet(@A,3,3,1);
 
 M := cvMat_(3,4,CV_32F,nil);
 cvCreateData(@M);

 IC := cvMat_(3,4,CV_32F,nil);
 cvCreateData(@IC);
 cvMInit(@IC,0) ;
 cvmSet(@IC,0,0,f );
 cvmSet(@IC,0,2,cx);
 cvmSet(@IC,1,1,f );
 cvmSet(@IC,1,2,cy);
 cvmSet(@IC,2,2,1 );

 cvMatMul(@IC,@A,@M);

 SetLength(MArr,3,4);
 cvmPrint(@M,MArr);

 InvM := cvMat_(4,3,CV_32F,nil);
 cvCreateData(@InvM);

 cvPseudoInverse(@M,@InvM);

 SetLength(InvMArr,4,3);
 cvmPrint(@InvM,InvMArr);

 
 //FindObjectPoints(InArr,OutArr,@InvM);


 FreeMem(objectPoints);
 FreeMem(imagePoints);
 FreeMem(focalLength);
 FreeMem(distortion);
 FreeMem(rotVect);
 FreeMem(transVect);

 cvReleaseData(@jaco);
 cvReleaseData(@rotation);
 cvReleaseData(@rotationvect);
 cvReleaseData(@A);
 cvReleaseData(@M);
 cvReleaseData(@IC);
 cvReleaseData(@InvM);
 
end;

Procedure cvMPrint( M : PCvMat; Arr : T2DPoint1fArr );
Var
 i,j : integer;
 pos : integer;
begin
 for j:=0 to M.cols-1 do
     for i:=0 to M.rows-1 do begin
         //pos:=i*M.cols+j;
         //Arr[pos]:=cvmGet(M,i,j);
         Arr[i,j]:=cvmGet(M,i,j);
     end;
end;

Procedure cvMInit( M : PCvMat; val : single );
Var
 i,j : integer;
 pos : integer;
begin
 for j:=0 to M.cols-1 do
     for i:=0 to M.rows-1 do begin
         pos:=i*M.cols+j;
         cvmSet(M,i,j,val);
     end;
end;



Function GetAngles(  RotArr : T2DPoint1fArr; Var a, b, g : single) : Boolean;
Var
 i,j      : integer;
 d,s      : extended;
 Up,Down  : extended;
begin
 result:=false;
 Up:=RotArr[1,0]-RotArr[0,1];
 Down:=RotArr[1,1]+RotArr[0,0];
 {s:=ArcTan2(Up,Down);}
 s:=ArcTan(Up/Down);
 Up:=RotArr[1,0]+RotArr[0,1];
 Down:=RotArr[1,1]-RotArr[0,0];
 d:=ArcTan(Up/Down);
 a:=(s+d)/2;
 g:=(s-d)/2;
 Up:=RotArr[0,2]*cos(a)+RotArr[1,2]*sin(a);
 Down:=RotArr[2,2];
 b:=ArcTan(Up/Down);
 a:=RadToDeg(a);
 b:=RadToDeg(b);
 g:=RadToDeg(g);
 result:=true;
end;


Procedure FindObjectPoints( InArr : T2DPoint3fArr; OutArr : T2DPoint3fArr;  M : PCvArr );
Var
 i,j    : integer;
 X,xx   : TCvMat;
 p3f    : TPoint3f;
 w      : Single;
begin
{
 X:=cvMat_(4,1,CV_32F,nil);
 cvCreateData(@X);

 xx:=cvMat_(3,1,CV_32F,nil);
 cvCreateData(@xx);

 //инициализация OUT массива
 p3f.x:=0; p3f.y:=0; p3f.z:=0;
 SetLength( OutArr, High(InArr), High(InArr[0]) );
 for i:=0 to High(OutArr) do
     for j:=0 to High(OutArr[i]) do 
         OutArr[i,j]:=p3f;

 for i:=0 to High(InArr) do
     for j:=0 to High(InArr[i]) do begin
         cvMInit(@xx,0);
         cvmSet(@xx,0,0,InArr[i,j].x);
         cvmSet(@xx,1,0,InArr[i,j].y);
         cvmSet(@xx,2,0,1);

         cvMInit(@X,0);
         cvMatMul(@InvM,@xx,@X);    
     
         w:=cvmGet(@X,0,0);
         OutArr[i,j].x:=cvmGet(@X,0,0)/w;
         OutArr[i,j].y:=cvmGet(@X,1,0)/w;
         OutArr[i,j].z:=cvmGet(@X,2,0)/w;
     end;
}
end;

end.

