/**
* OptimizePair:
* Input:
* cam1 - the first camera (already optimized)
* cam2 - the second camera with its intrinsic matrix initialized
* dR - an initial relative 3x3 camera rotation matrix
* set1 - SIFT features in the first image
* set2 - SIFT features in the second image
* aryInlier - the homography iniliers
*
* Ouput:
* cam2 - update cam2's optimized focal length and pose
*/
void OptimizeSingle( const CCamera& cam1, CCamera& cam2,
double* dR,
const CFeatureArray& set1,
const CFeatureArray& set2,
const MatchArray& aryInlier )
{
// Step 1. Initialize the camera pose of cam2
// cam2's relative rotation to cam1
CvMat matR = cvMat( 3, 3, CV_64F, dR );
// cam1's absolute rotation
double dRod1[3];
CvMat matRod1 = cvMat( 3, 1, CV_64F, dRod1 );
cam1.GetPose( dRod1 );
double dRot1[9];
CvMat matRot1 = cvMat( 3, 3, CV_64F, dRot1 );
cvRodrigues2( &matRod1, &matRot1 );
// compose R and Rot1 to get cam2's initial absolute rotation
cvMatMul( &matR, &matRot1, &matR );
double dRod2[3];
CvMat matRod2 = cvMat( 3, 1, CV_64F, dRod2 );
cvRodrigues2( &matR, &matRod2 );
cam2.SetPose( dRod2 );
// Step 2. Now we can perform bundle adjustment for cam2
CBundleAdjust ba( 1, BA_ITER );
ba.SetCamera( &cam2, 0 );
// set points
for( int i=0; i<aryInlier.size(); ++i )
{
const CFeature* ft1 = set1[ aryInlier[i].first ];
const CFeature* ft2 = set2[ aryInlier[i].second ];
double dir[3];
cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) );
// the 3d position
CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius );
ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft2->x, ft2->y ) );
}
ba.DoMotion();
ba.GetAdjustedCamera( &cam2, 0 );
}
/**
* OptimizePair:
* Input:
* cam1 - the first camera with its intrinsic matrix and pose initialized ([R|T]=[I|0])
* cam2 - the second camera with its intrinsic matrix initialized
* dR - an initial relative 3x3 camera rotation matrix
* set1 - SIFT features in the first image
* set2 - SIFT features in the second image
* aryInlier - the homography iniliers
*
* Ouput:
* cam1 - update cam1's optimized folcal length
* cam2 - update cam2's optimized focal length and pose
*/
void OptimizePair( CCamera& cam1, CCamera& cam2,
double* dR,
const CFeatureArray& set1,
const CFeatureArray& set2,
const MatchArray& aryInlier )
{
CBundleAdjust ba( 2, BA_ITER );
// Step 1. To perform bundle adjustment, we initialize cam1 and cam2
// as [K][I|0} and [K][R|0] respectively and optimize R using
// bundle adjustment.
double dRod[3];
CvMat matRod = cvMat( 3, 1, CV_64F, dRod );
CvMat matR = cvMat( 3, 3, CV_64F, dR );
cvRodrigues2( &matR, &matRod );
cam2.SetPose( dRod );
// Set cameras
ba.SetCamera( &cam1, 0 );
ba.SetCamera( &cam2, 1 );
// Step 2. We still need to create a set of 3D points. From each homography inlier,
// a 3D point can be initialized by locating it on the ray that goes through
// its projection.
for( int i=0; i<aryInlier.size(); ++i )
{
const CFeature* ft1 = set1[ aryInlier[i].first ];
const CFeature* ft2 = set2[ aryInlier[i].second ];
double dir[3];
cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) );
// the initialized 3d position
CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius );
// set the 3d point and its projections in both images
ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft1->x, ft1->y ) );
ba.SetPointProjection( pt3, 1, cvPoint2D64f( ft2->x, ft2->y ) );
}
// perform bundle adjustment
ba.DoMotionAndStructure();
// retrieve the optimized cameras
ba.GetAdjustedCamera( &cam1, 0 );
ba.GetAdjustedCamera( &cam2, 1 );
}
osg::Geode* myKeyboardEventHandler::Uncertainty()
{
osg::Geode* geode = new osg::Geode();
osg::Geometry* pointsGeom = new osg::Geometry();
osg::Vec3Array* vertices = new osg::Vec3Array;
CvMat *temp,*temp2,*temp3;
temp=cvCreateMatHeader(6,6,CV_32FC1);
CvMat *_temp = cvCreateMat (6,6,CV_32FC1);
temp2=cvCreateMat(3,6,CV_32FC1);
temp3=cvCreateMat(3,3,CV_32FC1);
CvMat* vect=cvCreateMat (6,1,CV_32FC1);
CvMat* res6=cvCreateMat (6,1,CV_32FC1);
//CvMat* vect2=cvCreateMat(1,6,CV_32FC1);
//CvPoint2D64f proj ;
//CvMat* m = cvCreateMat(3,1,CV_32FC1);
float *fstate;
fstate= pSlam->X->data.fl;
float xc,yc,zc,theta,phi,rho;
for (unsigned int i = 0 ; i < pSlam->visible.size(); i++)
{
if(pSlam->visible[i]==true)
{
xc=fstate[pSlam->modelSD+6*i];
yc=fstate[pSlam->modelSD+6*i+1];
zc=fstate[pSlam->modelSD+6*i+2];
theta=fstate[pSlam->modelSD+6*i+3];
phi=fstate[pSlam->modelSD+6*i+4];
rho=fstate[pSlam->modelSD+6*i+5];
cvGetSubRect( pSlam->P,temp,cvRect(pSlam->modelSD+i*6,pSlam->modelSD+i*6,6,6) );
for (int part=0; part<40; part++){
// cvmSet(vect,0,0,pSlam->randomVector(-.005,.005));
// cvmSet(vect,1,0,pSlam->randomVector(-.005,.005));
// cvmSet(vect,2,0,pSlam->randomVector(-.005,.005));
// cvmSet(vect,3,0,pSlam->randomVector(-.05,0.05));
// cvmSet(vect,4,0,pSlam->randomVector(-.05,0.05));
// cvmSet(vect,5,0,pSlam->randomVector(-1,1));
cvmSet(vect,0,0,pSlam->randomVector(-12.6,12.6));
cvmSet(vect,1,0,pSlam->randomVector(-12.6,12.6));
cvmSet(vect,2,0,pSlam->randomVector(-12.6,12.6));
cvmSet(vect,3,0,pSlam->randomVector(-12.6,12.6));
cvmSet(vect,4,0,pSlam->randomVector(-12.6,12.6));
cvmSet(vect,5,0,pSlam->randomVector(-12.6,12.6));
cvZero(_temp);
pSlam->Cholesky(temp,_temp);
cvMatMul(_temp,vect,res6);
double ox, oy, oz;
pSlam->InverseDepth2Depth(cvmGet(res6,0,0)+xc,
cvmGet(res6,1,0)+yc,
cvmGet(res6,2,0)+zc,
cvmGet(res6,3,0)+theta,
cvmGet(res6,4,0)+phi,
cvmGet(res6,5,0)+rho,
&ox,&oy,&oz );
//CvMat* nullmat = 0;
if (ox> -100000 && ox<10000 &&
oy> -100000 && oy<10000 &&
oz> -100000 && oz<10000)
{
CvPoint3D64f point = cvPoint3D64f(ox,oy,oz);
vertices->push_back(osg::Vec3(ox,oy,oz));
}
}
}
}
// pass the created vertex array to the points geometry object.
pointsGeom->setVertexArray(vertices);
// create the color of the geometry, one single for the whole geometry.
// for consistency of design even one single color must added as an element
// in a color array.
osg::Vec4Array* colors = new osg::Vec4Array;
// add a white color, colors take the form r,g,b,a with 0.0 off, 1.0 full on.
colors->push_back(osg::Vec4(1.0f,0.0f,0.0f,1.0f));
// pass the color array to points geometry, note the binding to tell the geometry
// that only use one color for the whole object.
pointsGeom->setColorArray(colors);
pointsGeom->setColorBinding(osg::Geometry::BIND_OVERALL);
// set the normal in the same way color.
osg::Vec3Array* normals = new osg::Vec3Array;
normals->push_back(osg::Vec3(0.0f,-1.0f,0.0f));
pointsGeom->setNormalArray(normals);
pointsGeom->setNormalBinding(osg::Geometry::BIND_OVERALL);
// create and add a DrawArray Primitive (see include/osg/Primitive). The first
// parameter passed to the DrawArrays constructor is the Primitive::Mode which
// in this case is POINTS (which has the same value GL_POINTS), the second
// parameter is the index position into the vertex array of the first point
// to draw, and the third parameter is the number of points to draw.
pointsGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,vertices->size()));
// add the points geometry to the geode.
geode->addDrawable(pointsGeom);
// Create a new StateSet with default settings:
osg::StateSet* stateOne = new osg::StateSet();
osg::Point * point = new osg::Point();
point->setSize(1.0);
stateOne->setAttributeAndModes(point,osg::StateAttribute::ON);
geode->setStateSet(stateOne);
return geode;
}
osg::Geode* myKeyboardEventHandler::PlotEllipse3D(CvMat *X, CvMat *P, osg::Vec4Array* colors )
{
osg::Geode* geode = new osg::Geode();
osg::Geometry* pointsGeom = new osg::Geometry();
osg::Vec3Array* vertices = new osg::Vec3Array;
CvMat *temp;
temp=cvCreateMatHeader(3,3,CV_32FC1);
CvMat *_temp = cvCreateMat (3,3,CV_32FC1);
CvMat* vect=cvCreateMat (3,1,CV_32FC1);
CvMat* res6=cvCreateMat (3,1,CV_32FC1);
//CvMat* vect2=cvCreateMat(1,6,CV_32FC1);
//CvPoint2D64f proj ;
//CvMat* m = cvCreateMat(3,1,CV_32FC1);
float *fstate;
fstate= X->data.fl;
float xc,yc,zc;
xc=fstate[0];
yc=fstate[1];
zc=fstate[2];
cvGetSubRect( P,temp,cvRect(0,0,3,3) );
// for (int part=0; part<100; part++){
for(float t = 0; t<2*3.14159; t+=0.1)
for (float p = -3.14159/2; p<3.14159;p+=0.1)
{
cvmSet(vect,0,0,7.8*cos(p)*cos(t));
cvmSet(vect,1,0,7.8*cos(p)*sin(t));
cvmSet(vect,2,0,7.8*sin(p));
cvZero(_temp);
pSlam->Cholesky(temp,_temp);
cvMatMul(_temp,vect,res6);
double ox=cvmGet(res6,0,0)+xc;
double oy=cvmGet(res6,1,0)+yc;
double oz=cvmGet(res6,2,0)+zc;
if (ox> -100000 && ox<10000 &&
oy> -100000 && oy<10000 &&
oz> -100000 && oz<10000)
{
CvPoint3D64f point = cvPoint3D64f(ox,oy,oz);
vertices->push_back(osg::Vec3(ox,oy,oz));
}
}
// pass the created vertex array to the points geometry object.
pointsGeom->setVertexArray(vertices);
// pass the color array to points geometry, note the binding to tell the geometry
// that only use one color for the whole object.
pointsGeom->setColorArray(colors);
pointsGeom->setColorBinding(osg::Geometry::BIND_OVERALL);
// set the normal in the same way color.
osg::Vec3Array* normals = new osg::Vec3Array;
normals->push_back(osg::Vec3(0.0f,-1.0f,0.0f));
pointsGeom->setNormalArray(normals);
pointsGeom->setNormalBinding(osg::Geometry::BIND_OVERALL);
// create and add a DrawArray Primitive (see include/osg/Primitive). The first
// parameter passed to the DrawArrays constructor is the Primitive::Mode which
// in this case is POINTS (which has the same value GL_POINTS), the second
// parameter is the index position into the vertex array of the first point
// to draw, and the third parameter is the number of points to draw.
pointsGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,vertices->size()));
// add the points geometry to the geode.
geode->addDrawable(pointsGeom);
// Create a new StateSet with default settings:
osg::StateSet* stateOne = new osg::StateSet();
osg::Point * point = new osg::Point();
point->setSize(1.0);
stateOne->setAttributeAndModes(point,osg::StateAttribute::ON);
geode->setStateSet(stateOne);
return geode;
}
void cvComputeRQDecomposition(CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, CvPoint3D64f *eulerAngles)
{
CvMat *tmpMatrix1 = 0;
CvMat *tmpMatrix2 = 0;
CvMat *tmpMatrixM = 0;
CvMat *tmpMatrixR = 0;
CvMat *tmpMatrixQ = 0;
CvMat *tmpMatrixQx = 0;
CvMat *tmpMatrixQy = 0;
CvMat *tmpMatrixQz = 0;
double tmpEulerAngleX, tmpEulerAngleY, tmpEulerAngleZ;
CV_FUNCNAME("cvRQDecomp3x3");
__CV_BEGIN__;
/* Validate parameters. */
if(matrixM == 0 || matrixR == 0 || matrixQ == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(matrixM) || !CV_IS_MAT(matrixR) || !CV_IS_MAT(matrixQ))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(matrixM->cols != 3 || matrixM->rows != 3 || matrixR->cols != 3 || matrixR->rows != 3 || matrixQ->cols != 3 || matrixQ->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
CV_CALL(tmpMatrix1 = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrix2 = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixR = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQ = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQx = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQy = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQz = cvCreateMat(3, 3, CV_64F));
cvCopy(matrixM, tmpMatrixM);
/* Find Givens rotation Q_x for x axis. */
/*
( 1 0 0 )
Qx = ( 0 c -s ), cos = -m33/sqrt(m32^2 + m33^2), cos = m32/sqrt(m32^2 + m33^2)
( 0 s c )
*/
double x, y, z, c, s;
x = cvmGet(tmpMatrixM, 2, 1);
y = cvmGet(tmpMatrixM, 2, 2);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = -y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQx);
cvmSet(tmpMatrixQx, 1, 1, c);
cvmSet(tmpMatrixQx, 1, 2, -s);
cvmSet(tmpMatrixQx, 2, 1, s);
cvmSet(tmpMatrixQx, 2, 2, c);
tmpEulerAngleX = acos(c) * 180.0 / CV_PI;
/* Multiply M on the right by Q_x. */
cvMatMul(tmpMatrixM, tmpMatrixQx, tmpMatrixR);
cvCopy(tmpMatrixR, tmpMatrixM);
assert(cvmGet(tmpMatrixM, 2, 1) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixM, 2, 1) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixM, 2, 1) != 0.0)
cvmSet(tmpMatrixM, 2, 1, 0.0); // Rectify arithmetic precision error.
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), cos = m33/sqrt(m31^2 + m33^2), cos = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
x = cvmGet(tmpMatrixM, 2, 0);
y = cvmGet(tmpMatrixM, 2, 2);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQy);
cvmSet(tmpMatrixQy, 0, 0, c);
cvmSet(tmpMatrixQy, 0, 2, s);
cvmSet(tmpMatrixQy, 2, 0, -s);
cvmSet(tmpMatrixQy, 2, 2, c);
tmpEulerAngleY = acos(c) * 180.0 / CV_PI;
/* Multiply M*Q_x on the right by Q_y. */
cvMatMul(tmpMatrixM, tmpMatrixQy, tmpMatrixR);
cvCopy(tmpMatrixR, tmpMatrixM);
assert(cvmGet(tmpMatrixM, 2, 0) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixM, 2, 0) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixM, 2, 0) != 0.0)
cvmSet(tmpMatrixM, 2, 0, 0.0); // Rectify arithmetic precision error.
/* Find Givens rotation for z axis. */
/*
( c -s 0 )
Qz = ( s c 0 ), cos = -m22/sqrt(m21^2 + m22^2), cos = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
x = cvmGet(tmpMatrixM, 1, 0);
y = cvmGet(tmpMatrixM, 1, 1);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = -y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQz);
cvmSet(tmpMatrixQz, 0, 0, c);
cvmSet(tmpMatrixQz, 0, 1, -s);
cvmSet(tmpMatrixQz, 1, 0, s);
cvmSet(tmpMatrixQz, 1, 1, c);
tmpEulerAngleZ = acos(c) * 180.0 / CV_PI;
/* Multiply M*Q_x*Q_y on the right by Q_z. */
cvMatMul(tmpMatrixM, tmpMatrixQz, tmpMatrixR);
assert(cvmGet(tmpMatrixR, 1, 0) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixR, 1, 0) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixR, 1, 0) != 0.0)
cvmSet(tmpMatrixR, 1, 0, 0.0); // Rectify arithmetic precision error.
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvTranspose(tmpMatrixQz, tmpMatrix1);
cvTranspose(tmpMatrixQy, tmpMatrix2);
cvMatMul(tmpMatrix1, tmpMatrix2, tmpMatrixQ);
cvCopy(tmpMatrixQ, tmpMatrix1);
cvTranspose(tmpMatrixQx, tmpMatrix2);
cvMatMul(tmpMatrix1, tmpMatrix2, tmpMatrixQ);
/* Solve decomposition ambiguity. */
/*
Diagonal entries of R should be positive, so swap signs if necessary.
*/
if(cvmGet(tmpMatrixR, 0, 0) < 0.0) {
cvmSet(tmpMatrixR, 0, 0, -1.0 * cvmGet(tmpMatrixR, 0, 0));
cvmSet(tmpMatrixQ, 0, 0, -1.0 * cvmGet(tmpMatrixQ, 0, 0));
cvmSet(tmpMatrixQ, 0, 1, -1.0 * cvmGet(tmpMatrixQ, 0, 1));
cvmSet(tmpMatrixQ, 0, 2, -1.0 * cvmGet(tmpMatrixQ, 0, 2));
}
if(cvmGet(tmpMatrixR, 1, 1) < 0.0) {
cvmSet(tmpMatrixR, 0, 1, -1.0 * cvmGet(tmpMatrixR, 0, 1));
cvmSet(tmpMatrixR, 1, 1, -1.0 * cvmGet(tmpMatrixR, 1, 1));
cvmSet(tmpMatrixQ, 1, 0, -1.0 * cvmGet(tmpMatrixQ, 1, 0));
cvmSet(tmpMatrixQ, 1, 1, -1.0 * cvmGet(tmpMatrixQ, 1, 1));
cvmSet(tmpMatrixQ, 1, 2, -1.0 * cvmGet(tmpMatrixQ, 1, 2));
}
/* Enforce det(Q) = 1 */
if (cvDet(tmpMatrixQ) < 0)
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
cvmSet(tmpMatrixQ, j, i, -cvmGet(tmpMatrixQ, j, i));
}
}
}
/* Save R and Q matrices. */
cvCopy(tmpMatrixR, matrixR);
cvCopy(tmpMatrixQ, matrixQ);
if(matrixQx && CV_IS_MAT(matrixQx) && matrixQx->cols == 3 || matrixQx->rows == 3)
cvCopy(tmpMatrixQx, matrixQx);
if(matrixQy && CV_IS_MAT(matrixQy) && matrixQy->cols == 3 || matrixQy->rows == 3)
cvCopy(tmpMatrixQy, matrixQy);
if(matrixQz && CV_IS_MAT(matrixQz) && matrixQz->cols == 3 || matrixQz->rows == 3)
cvCopy(tmpMatrixQz, matrixQz);
/* Save Euler angles. */
if(eulerAngles)
*eulerAngles = cvPoint3D64f(tmpEulerAngleX, tmpEulerAngleY, tmpEulerAngleZ);
__CV_END__;
cvReleaseMat(&tmpMatrix1);
cvReleaseMat(&tmpMatrix2);
cvReleaseMat(&tmpMatrixM);
cvReleaseMat(&tmpMatrixR);
cvReleaseMat(&tmpMatrixQ);
cvReleaseMat(&tmpMatrixQx);
cvReleaseMat(&tmpMatrixQy);
cvReleaseMat(&tmpMatrixQz);
}
