int main()
{
QueueCV<int> queue;
Producer p(queue);
Consumer c0(queue, 10);
Consumer c1(queue, 11);
Consumer c2(queue, 12);
std::thread tC0(&Consumer::run, c0);
std::thread tC1(&Consumer::run, c1);
std::thread tC2(&Consumer::run, c2);
std::thread tP(&Producer::run, p);
std::vector<std::thread> arrC;
arrC.push_back( std::move(tC0));
arrC.push_back( std::move(tC1));
arrC.push_back( std::move(tC2));
tP.join();
for (auto &c : arrC)
if (c.joinable())
c.join();
return 1;
}
inline void
SimpleSingularValuesUpper
( DistMatrix<Complex<Real> >& A,
DistMatrix<Real,VR,STAR>& s )
{
#ifndef RELEASE
PushCallStack("svd::SimpleSingularValuesUpper");
#endif
typedef Complex<Real> C;
const int m = A.Height();
const int n = A.Width();
const int k = std::min( m, n );
const int offdiagonal = ( m>=n ? 1 : -1 );
const char uplo = ( m>=n ? 'U' : 'L' );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<C,STAR,STAR> tP(g), tQ(g);
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR( g ),
e_MD_STAR( g );
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// In order to use serial QR kernels, we need the full bidiagonal matrix
// on each process
//
// NOTE: lapack::BidiagQRAlg expects e to be of length k
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR );
DistMatrix<Real,STAR,STAR> eHat_STAR_STAR( k, 1, g );
DistMatrix<Real,STAR,STAR> e_STAR_STAR( g );
e_STAR_STAR.View( eHat_STAR_STAR, 0, 0, k-1, 1 );
e_STAR_STAR = e_MD_STAR;
// Compute the singular values of the bidiagonal matrix
lapack::BidiagQRAlg
( uplo, k, 0, 0,
d_STAR_STAR.LocalBuffer(), e_STAR_STAR.LocalBuffer(),
(C*)0, 1, (C*)0, 1 );
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
GolubReinschUpper
( DistMatrix<F>& A,
DistMatrix<BASE(F),VR,STAR>& s )
{
#ifndef RELEASE
CallStackEntry entry("svd::GolubReinschUpper");
#endif
typedef BASE(F) Real;
const Int m = A.Height();
const Int n = A.Width();
const Int k = Min( m, n );
const Int offdiagonal = ( m>=n ? 1 : -1 );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<F,STAR,STAR> tP(g), tQ(g);
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR(g), e_MD_STAR(g);
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// In order to use serial DQDS kernels, we need the full bidiagonal matrix
// on each process
//
// NOTE: lapack::BidiagDQDS expects e to be of length k
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR ),
eHat_STAR_STAR( k, 1, g ),
e_STAR_STAR( g );
View( e_STAR_STAR, eHat_STAR_STAR, 0, 0, k-1, 1 );
e_STAR_STAR = e_MD_STAR;
// Compute the singular values of the bidiagonal matrix via DQDS
lapack::BidiagDQDS( k, d_STAR_STAR.Buffer(), e_STAR_STAR.Buffer() );
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
}
void p_wid::paintEvent(QPaintEvent *event)
{
// Struct_poz s = this->getSelectionBoundings();
QPainter pain( this );
//drawing the bg-MUSTER
for ( unsigned int i = 0; i < this->current_tilesets_height; i ++)
pain.drawPixmap( 0, 32*i, 256, 32, QPixmap( ":/resources/img/p_wid_background_rib.png" ) );
QPixmap tmp_pixmap = this->tilesets.at( this->current_tileset );
pain.drawPixmap(0, 0, 256, tmp_pixmap.height(), tmp_pixmap );
//*** now the advanced stuff
//drawing the grid
QImage template1 = QImage( 32,32, QImage::Format_ARGB32 );
template1.fill( 0 );
QPainter tP( &template1 );
tP.setPen( QColor( 100, 100, 100, 100 ) );
tP.drawLine( 0,0, 31,0 );
tP.drawLine( 0,0, 0,31 );
tP.drawLine( 0,31, 31,31 );
tP.drawLine( 31,0, 31,31 );
if ( this->right_click_menu->actions()[ 0 ]->isChecked() )
{
for ( int hz = 0; hz < this->tilesets[ this->current_tileset ].height(); hz += 32 )
{
for ( int w = 0; w < 8; w++ )
{
pain.drawImage( QPoint(w*32, hz), template1);
}
}
}
//drawing the dim
//framing the selection
Struct_poz rec;
rec.first_tile = 0;
rec = this->getSelectionBoundings();
// mid white rect
QRect frame;
frame.setY( ( rec.first_tile / 8 ) * 32 );
frame.setX( ( rec.first_tile % 8 ) * 32 );
frame.setWidth( rec.width * 32 );
frame.setHeight( rec.height * 32 );
pain.setPen( Qt::white );
pain.drawRect( frame );
// outa black frame
frame.setY( ( rec.first_tile / 8 ) * 32 -1);
frame.setX( ( rec.first_tile % 8 ) * 32 -1);
frame.setWidth( rec.width * 32 +2);
frame.setHeight( rec.height * 32 +2);
pain.setPen( Qt::black );
pain.drawRect( frame );
//inna black frame
frame.setY( ( rec.first_tile / 8 ) * 32 +1);
frame.setX( ( rec.first_tile % 8 ) * 32 +1);
frame.setWidth( rec.width * 32 -2);
frame.setHeight( rec.height * 32 -2);
pain.setPen( Qt::black );
pain.drawRect( frame );
pain.end();
event->accept();
}
inline void
GolubReinschUpper
( DistMatrix<F>& A, DistMatrix<BASE(F),VR,STAR>& s, DistMatrix<F>& V )
{
#ifndef RELEASE
CallStackEntry entry("svd::GolubReinschUpper");
#endif
typedef BASE(F) Real;
const Int m = A.Height();
const Int n = A.Width();
const Int k = Min( m, n );
const Int offdiagonal = ( m>=n ? 1 : -1 );
const char uplo = ( m>=n ? 'U' : 'L' );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<F,STAR,STAR> tP( g ), tQ( g );
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR(g), e_MD_STAR(g);
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// NOTE: lapack::BidiagQRAlg expects e to be of length k
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR ),
eHat_STAR_STAR( k, 1, g ),
e_STAR_STAR( g );
View( e_STAR_STAR, eHat_STAR_STAR, 0, 0, k-1, 1 );
e_STAR_STAR = e_MD_STAR;
// Initialize U and VAdj to the appropriate identity matrices
DistMatrix<F,VC,STAR> U_VC_STAR( g );
DistMatrix<F,STAR,VC> VAdj_STAR_VC( g );
U_VC_STAR.AlignWith( A );
VAdj_STAR_VC.AlignWith( V );
Identity( U_VC_STAR, m, k );
Identity( VAdj_STAR_VC, k, n );
// Compute the SVD of the bidiagonal matrix and accumulate the Givens
// rotations into our local portion of U and VAdj
Matrix<F>& ULoc = U_VC_STAR.Matrix();
Matrix<F>& VAdjLoc = VAdj_STAR_VC.Matrix();
lapack::BidiagQRAlg
( uplo, k, VAdjLoc.Width(), ULoc.Height(),
d_STAR_STAR.Buffer(), e_STAR_STAR.Buffer(),
VAdjLoc.Buffer(), VAdjLoc.LDim(),
ULoc.Buffer(), ULoc.LDim() );
// Make a copy of A (for the Householder vectors) and pull the necessary
// portions of U and VAdj into a standard matrix dist.
DistMatrix<F> B( A );
if( m >= n )
{
DistMatrix<F> AT(g), AB(g);
DistMatrix<F,VC,STAR> UT_VC_STAR(g), UB_VC_STAR(g);
PartitionDown( A, AT, AB, n );
PartitionDown( U_VC_STAR, UT_VC_STAR, UB_VC_STAR, n );
AT = UT_VC_STAR;
MakeZeros( AB );
Adjoint( VAdj_STAR_VC, V );
}
else
{
DistMatrix<F> VT(g), VB(g);
DistMatrix<F,STAR,VC> VAdjL_STAR_VC(g), VAdjR_STAR_VC(g);
PartitionDown( V, VT, VB, m );
PartitionRight( VAdj_STAR_VC, VAdjL_STAR_VC, VAdjR_STAR_VC, m );
Adjoint( VAdjL_STAR_VC, VT );
MakeZeros( VB );
}
// Backtransform U and V
bidiag::ApplyU( LEFT, NORMAL, B, tQ, A );
bidiag::ApplyV( LEFT, NORMAL, B, tP, V );
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
}
inline void
GolubReinschUpper_FLA
( DistMatrix<F>& A, DistMatrix<BASE(F),VR,STAR>& s, DistMatrix<F>& V )
{
#ifndef RELEASE
CallStackEntry entry("svd::GolubReinschUpper_FLA");
#endif
typedef BASE(F) Real;
const Int m = A.Height();
const Int n = A.Width();
const Int k = Min( m, n );
const Int offdiagonal = ( m>=n ? 1 : -1 );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<F,STAR,STAR> tP(g), tQ(g);
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR(g), e_MD_STAR(g);
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// In order to use serial QR kernels, we need the full bidiagonal matrix
// on each process
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR ),
e_STAR_STAR( e_MD_STAR );
// Initialize U and VAdj to the appropriate identity matrices
DistMatrix<F,VC,STAR> U_VC_STAR(g), V_VC_STAR(g);
U_VC_STAR.AlignWith( A );
V_VC_STAR.AlignWith( V );
Identity( U_VC_STAR, m, k );
Identity( V_VC_STAR, n, k );
FlaSVD
( k, U_VC_STAR.LocalHeight(), V_VC_STAR.LocalHeight(),
d_STAR_STAR.Buffer(), e_STAR_STAR.Buffer(),
U_VC_STAR.Buffer(), U_VC_STAR.LDim(),
V_VC_STAR.Buffer(), V_VC_STAR.LDim() );
// Make a copy of A (for the Householder vectors) and pull the necessary
// portions of U and V into a standard matrix dist.
DistMatrix<F> B( A );
if( m >= n )
{
DistMatrix<F> AT(g), AB(g);
DistMatrix<F,VC,STAR> UT_VC_STAR(g), UB_VC_STAR(g);
PartitionDown( A, AT, AB, n );
PartitionDown( U_VC_STAR, UT_VC_STAR, UB_VC_STAR, n );
AT = UT_VC_STAR;
MakeZeros( AB );
V = V_VC_STAR;
}
else
{
DistMatrix<F> VT(g), VB(g);
DistMatrix<F,VC,STAR> VT_VC_STAR(g), VB_VC_STAR(g);
PartitionDown( V, VT, VB, m );
PartitionDown( V_VC_STAR, VT_VC_STAR, VB_VC_STAR, m );
VT = VT_VC_STAR;
MakeZeros( VB );
}
// Backtransform U and V
bidiag::ApplyU( LEFT, NORMAL, B, tQ, A );
bidiag::ApplyV( LEFT, NORMAL, B, tP, V );
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
}
inline void
SimpleSVDUpper
( DistMatrix<Complex<double> >& A,
DistMatrix<double,VR,STAR>& s,
DistMatrix<Complex<double> >& V )
{
#ifndef RELEASE
PushCallStack("svd::SimpleSVDUpper");
#endif
typedef double Real;
typedef Complex<Real> C;
const int m = A.Height();
const int n = A.Width();
const int k = std::min( m, n );
const int offdiagonal = ( m>=n ? 1 : -1 );
const char uplo = ( m>=n ? 'U' : 'L' );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<C,STAR,STAR> tP( g ), tQ( g );
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR( g ),
e_MD_STAR( g );
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// In order to use serial QR kernels, we need the full bidiagonal matrix
// on each process
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR ),
e_STAR_STAR( e_MD_STAR );
// Initialize U and VAdj to the appropriate identity matrices
DistMatrix<C,VC,STAR> U_VC_STAR( g );
DistMatrix<C,VC,STAR> V_VC_STAR( g );
U_VC_STAR.AlignWith( A );
V_VC_STAR.AlignWith( V );
Identity( m, k, U_VC_STAR );
Identity( n, k, V_VC_STAR );
// Compute the SVD of the bidiagonal matrix and accumulate the Givens
// rotations into our local portion of U and V
// NOTE: This _only_ works in the case where m >= n
const int numAccum = 32;
const int maxNumIts = 30;
const int bAlg = 512;
std::vector<C> GBuffer( (k-1)*numAccum ),
HBuffer( (k-1)*numAccum );
FLA_Bsvd_v_opz_var1
( k, U_VC_STAR.LocalHeight(), V_VC_STAR.LocalHeight(),
numAccum, maxNumIts,
d_STAR_STAR.LocalBuffer(), 1,
e_STAR_STAR.LocalBuffer(), 1,
&GBuffer[0], 1, k-1,
&HBuffer[0], 1, k-1,
U_VC_STAR.LocalBuffer(), 1, U_VC_STAR.LocalLDim(),
V_VC_STAR.LocalBuffer(), 1, V_VC_STAR.LocalLDim(),
bAlg );
// Make a copy of A (for the Householder vectors) and pull the necessary
// portions of U and V into a standard matrix dist.
DistMatrix<C> B( A );
if( m >= n )
{
DistMatrix<C> AT( g ),
AB( g );
DistMatrix<C,VC,STAR> UT_VC_STAR( g ),
UB_VC_STAR( g );
PartitionDown( A, AT,
AB, n );
PartitionDown( U_VC_STAR, UT_VC_STAR,
UB_VC_STAR, n );
AT = UT_VC_STAR;
MakeZeros( AB );
V = V_VC_STAR;
}
else
{
DistMatrix<C> VT( g ),
VB( g );
DistMatrix<C,VC,STAR> VT_VC_STAR( g ),
VB_VC_STAR( g );
PartitionDown( V, VT,
VB, m );
PartitionDown
( V_VC_STAR, VT_VC_STAR,
VB_VC_STAR, m );
VT = VT_VC_STAR;
MakeZeros( VB );
}
// Backtransform U and V
if( m >= n )
{
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, BACKWARD, UNCONJUGATED, 0, B, tQ, A );
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, BACKWARD, UNCONJUGATED, 1, B, tP, V );
}
else
{
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, BACKWARD, UNCONJUGATED, -1, B, tQ, A );
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, BACKWARD, UNCONJUGATED, 0, B, tP, V );
}
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
SimpleSVDUpper
( DistMatrix<Complex<Real> >& A,
DistMatrix<Real,VR,STAR>& s,
DistMatrix<Complex<Real> >& V )
{
#ifndef RELEASE
PushCallStack("svd::SimpleSVDUpper");
#endif
typedef Complex<Real> C;
const int m = A.Height();
const int n = A.Width();
const int k = std::min( m, n );
const int offdiagonal = ( m>=n ? 1 : -1 );
const char uplo = ( m>=n ? 'U' : 'L' );
const Grid& g = A.Grid();
// Bidiagonalize A
DistMatrix<C,STAR,STAR> tP( g ), tQ( g );
Bidiag( A, tP, tQ );
// Grab copies of the diagonal and sub/super-diagonal of A
DistMatrix<Real,MD,STAR> d_MD_STAR( g ),
e_MD_STAR( g );
A.GetRealPartOfDiagonal( d_MD_STAR );
A.GetRealPartOfDiagonal( e_MD_STAR, offdiagonal );
// NOTE: lapack::BidiagQRAlg expects e to be of length k
DistMatrix<Real,STAR,STAR> d_STAR_STAR( d_MD_STAR );
DistMatrix<Real,STAR,STAR> eHat_STAR_STAR( k, 1, g );
DistMatrix<Real,STAR,STAR> e_STAR_STAR( g );
e_STAR_STAR.View( eHat_STAR_STAR, 0, 0, k-1, 1 );
e_STAR_STAR = e_MD_STAR;
// Initialize U and VAdj to the appropriate identity matrices
DistMatrix<C,VC,STAR> U_VC_STAR( g );
DistMatrix<C,STAR,VC> VAdj_STAR_VC( g );
U_VC_STAR.AlignWith( A );
VAdj_STAR_VC.AlignWith( V );
Identity( m, k, U_VC_STAR );
Identity( k, n, VAdj_STAR_VC );
// Compute the SVD of the bidiagonal matrix and accumulate the Givens
// rotations into our local portion of U and VAdj
Matrix<C>& ULocal = U_VC_STAR.LocalMatrix();
Matrix<C>& VAdjLocal = VAdj_STAR_VC.LocalMatrix();
lapack::BidiagQRAlg
( uplo, k, VAdjLocal.Width(), ULocal.Height(),
d_STAR_STAR.LocalBuffer(), e_STAR_STAR.LocalBuffer(),
VAdjLocal.Buffer(), VAdjLocal.LDim(),
ULocal.Buffer(), ULocal.LDim() );
// Make a copy of A (for the Householder vectors) and pull the necessary
// portions of U and VAdj into a standard matrix dist.
DistMatrix<C> B( A );
if( m >= n )
{
DistMatrix<C> AT( g ),
AB( g );
DistMatrix<C,VC,STAR> UT_VC_STAR( g ),
UB_VC_STAR( g );
PartitionDown( A, AT,
AB, n );
PartitionDown( U_VC_STAR, UT_VC_STAR,
UB_VC_STAR, n );
AT = UT_VC_STAR;
MakeZeros( AB );
Adjoint( VAdj_STAR_VC, V );
}
else
{
DistMatrix<C> VT( g ),
VB( g );
DistMatrix<C,STAR,VC> VAdjL_STAR_VC( g ), VAdjR_STAR_VC( g );
PartitionDown( V, VT,
VB, m );
PartitionRight( VAdj_STAR_VC, VAdjL_STAR_VC, VAdjR_STAR_VC, m );
Adjoint( VAdjL_STAR_VC, VT );
MakeZeros( VB );
}
// Backtransform U and V
if( m >= n )
{
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, BACKWARD, UNCONJUGATED, 0, B, tQ, A );
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, BACKWARD, UNCONJUGATED, 1, B, tP, V );
}
else
{
ApplyPackedReflectors
( LEFT, LOWER, VERTICAL, BACKWARD, UNCONJUGATED, -1, B, tQ, A );
ApplyPackedReflectors
( LEFT, UPPER, HORIZONTAL, BACKWARD, UNCONJUGATED, 0, B, tP, V );
}
// Copy out the appropriate subset of the singular values
s = d_STAR_STAR;
#ifndef RELEASE
PopCallStack();
#endif
}
/**
* @brief Calculates the view point of the newFrame relative to the oldFrames using pcl::estimateRigidTransformationSVD and RANSAC.
* @return True if a good position has been found.
*/
bool utils::pclRigidTransform(const std::vector<KeyframeContainer>& oldFrames,KeyframeContainer& newFrame)
{
//fill point clouds with all matches and another set with the best matches
pcl::PointCloud<pcl::PointXYZ> cloud1,cloud2;
std::vector<cv::Mat_<double> > points2D,points3D;
pcl::PointCloud<pcl::PointXYZ> completeCloud1,completeCloud2;
std::vector<cv::Mat_<double> > completePoints2D,completePoints3D;
//precalculate the extended projection matrices
cv::Mat_<double> proj2(3,4,0.0);
std::vector<cv::Mat_<double> > proj1Collected;
for(uint i=0;i<oldFrames.size();++i)
{
cv::Mat_<double> proj1E;
if(oldFrames[i].projectionMatrix.rows>0 && oldFrames[i].projectionMatrix.cols>0)
{
proj1E=cv::Mat_<double>(4,4,0.0);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj1E(x,y)=oldFrames[i].projectionMatrix(x,y);
proj1E(3,3)=1;
}
proj1Collected.push_back(proj1E);
}
//save all the matched points and the best matches
for(uint me=0;me<newFrame.matches.size();++me)
{
//search the best match to the current best match by comparing match quality
uint currentBestMatchIndex=0;
double currentBestQuality=INFINITY;
uint tmpIndex=0;
for(uint you=0;you<newFrame.matches[me].size();++you)
{
if(currentBestQuality>newFrame.matches[me][you].second)
{
//find the Frame that the keypoint belongs to
tmpIndex=0;
while(oldFrames[tmpIndex].ID!=newFrame.matches[me][you].first.val[0])
++tmpIndex;
//if this frame hasn't been flagged invalid use this keypoint
if(!oldFrames[tmpIndex].invalid)
{
currentBestMatchIndex=you;
currentBestQuality=newFrame.matches[me][you].second;
}
}
}
//calculate all the points from the matches
for(uint you=0;you<newFrame.matches[me].size();++you)
{
//we only know the ID of the frame and need to find the index
uint currentFrame1Index=0;
while(oldFrames[currentFrame1Index].ID!=newFrame.matches[me][you].first.val[0])
++currentFrame1Index;
//add points only if the frame is valid
if(!oldFrames[currentFrame1Index].invalid)
{
//get the 2D points from both frames
cv::Point2f p1=oldFrames[currentFrame1Index].keypoints[newFrame.matches[me][you].first.val[1]].pt;
cv::Point2f p2=newFrame.keypoints[me].pt;
//get the depth information from both frames
float depth1=oldFrames[currentFrame1Index].depthImg.image.at<float>(p1.y,p1.x);
float depth2=newFrame.depthImg.image.at<float>(p2.y,p2.x);
//if both depths are available and the projectivity matrix is valid
if(isnan(depth1)==0 && isnan(depth2)==0 && proj1Collected[currentFrame1Index].rows>0 && proj1Collected[currentFrame1Index].cols>0)
{
//calculate the 2D and 3D point of the first frame
pcl::PointXYZ tmpPoint1;
cv::Mat_<double> tmpPoint2D(2,1,0.0);
tmpPoint2D(0,0)=p1.x;
tmpPoint2D(1,0)=p1.y;
cv::Mat_<double> tmpPointHomo3D=reprojectImagePointTo3D(tmpPoint2D,oldFrames[currentFrame1Index].cameraCalibration,proj1Collected[currentFrame1Index],depth1);
//convert 3D point to pcl format
tmpPoint1.x=tmpPointHomo3D(0,0);
tmpPoint1.y=tmpPointHomo3D(1,0);
tmpPoint1.z=tmpPointHomo3D(2,0);
//calculate the 3D point of the second frame
pcl::PointXYZ tmpPoint2;
cv::Mat_<double> tmpP2D(3,1,1.0);
tmpP2D(0,0)=p2.x;
tmpP2D(1,0)=p2.y;
tmpP2D=utils::reprojectImagePointTo2DNormalised(tmpP2D,newFrame.cameraCalibration);
//convert 3D point to pcl format
tmpPoint2.x=tmpP2D(0,0)*depth2;
tmpPoint2.y=tmpP2D(1,0)*depth2;
tmpPoint2.z=depth2;
//put the match in the complete point cloud
completeCloud1.push_back(tmpPoint1);
completePoints3D.push_back(tmpPointHomo3D);
completeCloud2.push_back(tmpPoint2);
completePoints2D.push_back(tmpP2D);
//if you is the best match put it into the corresponding clouds
if(you==currentBestMatchIndex)
{
cloud1.push_back(tmpPoint1);
points3D.push_back(tmpPointHomo3D);
cloud2.push_back(tmpPoint2);
points2D.push_back(tmpP2D);
}
}
}
}
}
#ifndef NDEBUG
ROS_INFO("cloud size for ransac: %lu",cloud1.points.size());
#endif
//RANSAC only if there are some points available
if(cloud1.size()>20)
{
Eigen::Matrix4f transformation;
const double REPROJECTION_THRESHOLD_SQUARE=1e-4;
const double DESIRED_CONFIDENCE_LOG=std::log(0.01);
const int MAX_ITERATIONS=200;
double iterationsNeeded=MAX_ITERATIONS;
int iteration=0;
std::vector<int> inliers;
std::vector<int> bestInliers;
std::vector<int> randomIndices;
cv::RNG Rander(std::time(NULL));
cv::Mat_<double> tP;
cv::Mat_<double> proj2(3,4,0.0);
//do adaptive RANSAC on the best match clouds
while((double)iteration<std::ceil(iterationsNeeded))
{
//get random indices
randomIndices.clear();
while(randomIndices.size()<3)
{
int newIndex=Rander.uniform(0,(int) cloud1.points.size());
for(uint ind=0;ind<randomIndices.size();ind++)
if(randomIndices[ind]==newIndex) newIndex=-1;
if(newIndex>=0) randomIndices.push_back(newIndex);
}
//estimate the transform
pcl::estimateRigidTransformationSVD(cloud1,randomIndices,cloud2,randomIndices,transformation);
//convert projection matrix to opencv format
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
//estimate the inliers
inliers.clear();
for(int i=0;i<(int)points3D.size();i++)
{
tP=proj2*points3D[i];
tP/=tP(2,0);
tP-=points2D[i];
if(tP(0,0)*tP(0,0)+tP(1,0)*tP(1,0)<REPROJECTION_THRESHOLD_SQUARE)
inliers.push_back(i);
}
//save the solution if it's better than before
if(inliers.size()>bestInliers.size())
{
bestInliers=inliers;
iterationsNeeded=DESIRED_CONFIDENCE_LOG/std::log(1-std::pow(((double)bestInliers.size())/((double)cloud1.points.size()),3));
}
iteration++;
}
#ifndef NDEBUG
ROS_INFO("inlier ratio: %f",((double)bestInliers.size())/((double)cloud1.points.size()));
#endif
//calculate final solution from the complete point clouds only if enough inliers have been found and the inlier ration is high enough
if(bestInliers.size()>9 && ((double)bestInliers.size())/((double)cloud1.points.size())>0.1)
{
//reestimate the best transformation from the reduced point cloud
pcl::estimateRigidTransformationSVD(cloud1,bestInliers,cloud2,bestInliers,transformation);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
//find all inliers in the complete point cloud
inliers.clear();
for(int i=0;i<(int)completePoints3D.size();i++)
{
tP=proj2*completePoints3D[i];
tP/=tP(2,0);
tP-=completePoints2D[i];
if(tP(0,0)*tP(0,0)+tP(1,0)*tP(1,0)<REPROJECTION_THRESHOLD_SQUARE)
inliers.push_back(i);
}
//estimate the transform from the complete cloud
pcl::estimateRigidTransformationSVD(completeCloud1,inliers,completeCloud2,inliers,transformation);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
newFrame.projectionMatrix=proj2;
return true;
}
}
return false;
}
