Eigen::Vector3d intersectRayPlane(const Eigen::Vector3d ray,
const Eigen::Vector3d ray_origin, const Eigen::Vector4d plane)
{
// Plane defined as ax + by + cz + d = 0
// Ray: P = P0 + tV
// Plane: P.N + d = 0, where P is intersection point
// t = -(P0.N + d)/(V.N) , P = P0 + tV
float t = -(ray_origin.dot(plane.head(3)) + plane[3]) / (ray.dot(plane.head(3)));
Eigen::Vector3d P = ray_origin + t*ray;
return P;
}
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
//ds A matrix for system: A*X=0
Eigen::Matrix4d matAHomogeneous;
matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);
//ds solve system subject to ||A*x||=0 ||x||=1
const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );
//ds solution x is the last column of V
const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );
return vecX.head( 3 )/vecX(3);
}
void LinearSystem::_BuildSystem(
LinearSystem* pLS,
const unsigned int& StartU,
const unsigned int& EndU,
const unsigned int& StartV,
const unsigned int& EndV
)
{
// Jacobian
Eigen::Matrix<double, 1, 6> BigJ;
Eigen::Matrix<double, 1, 6> J;
BigJ.setZero();
J.setZero();
// Errors
double e;
double SSD = 0;
double Error = 0;
unsigned int ErrorPts = 0;
for( int ii = StartV; ii < EndV; ii++ ) {
for( int jj = StartU; jj < EndU; jj++ ) {
// variables
Eigen::Vector2d Ur; // pixel position
Eigen::Vector3d Pr, Pv; // 3d point
Eigen::Vector4d Ph; // homogenized point
// check if pixel is contained in our model (i.e. has depth)
if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
continue;
}
// --------------------- first term 1x2
// evaluate 'a' = L[ Trv * Linv( Uv ) ]
// back project to virtual camera's reference frame
// this already brings points to robotics reference frame
Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );
// convert to homogeneous coordinate
Ph << Pv, 1;
// transform point to reference camera's frame
// Pr = Trv * Pv
Ph = pLS->m_dTrv * Ph;
Pr = Ph.head( 3 );
// project onto reference camera
Eigen::Vector3d Lr;
Lr = pLS->_Project( Pr );
Ur = Lr.head( 2 );
Ur = Ur / Lr( 2 );
// check if point falls in camera's field of view
if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
|| (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
continue;
}
// finite differences
float TopPix = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
float BotPix = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
float LeftPix = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
float RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
Eigen::Matrix<double, 1, 2> Term1;
Term1( 0 ) = (RightPix - LeftPix) / 2.0;
Term1( 1 ) = (BotPix - TopPix) / 2.0;
// --------------------- second term 2x3
// evaluate 'b' = Trv * Linv( Uv )
// this was already calculated for Term1
// fill matrix
// 1/c 0 -a/c^2
// 0 1/c -b/c^2
Eigen::Matrix<double, 2, 3> Term2;
double PowC = Lr( 2 ) * Lr( 2 );
Term2( 0, 0 ) = 1.0 / Lr( 2 );
Term2( 0, 1 ) = 0;
Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
Term2( 1, 0 ) = 0;
Term2( 1, 1 ) = 1.0 / Lr( 2 );
Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
Term2 = Term2 * pLS->m_Kr;
// --------------------- third term 3x1
// we need Pv in homogenous coordinates
Ph << Pv, 1;
Eigen::Vector4d Term3i;
// last row of Term3 is truncated since it is always 0
Eigen::Vector3d Term3;
// fill Jacobian with T generators
Term3i = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 0 ) = Term1 * Term2 * Term3;
pLS->m_vJImgX[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 0);
Term3i = pLS->m_dTrv * pLS->m_Gen[1] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 1 ) = Term1 * Term2 * Term3;
pLS->m_vJImgY[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 1);
Term3i = pLS->m_dTrv * pLS->m_Gen[2] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 2 ) = Term1 * Term2 * Term3;
pLS->m_vJImgZ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 2);
Term3i = pLS->m_dTrv * pLS->m_Gen[3] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 3 ) = Term1 * Term2 * Term3;
pLS->m_vJImgP[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 3);
Term3i = pLS->m_dTrv * pLS->m_Gen[4] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 4 ) = Term1 * Term2 * Term3;
pLS->m_vJImgQ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 4);
Term3i = pLS->m_dTrv * pLS->m_Gen[5] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 5 ) = Term1 * Term2 * Term3;
pLS->m_vJImgR[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 5);
// accumulate Jacobian
BigJ += J;
// accumulate SSD error
e = pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
- pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj];
SSD += e * e;
// calculate normalized error
Error += fabs( pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
- pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj] );
ErrorPts++;
}
}
// update global LHS and RHS
// ---------- start contention zone
pLS->m_Mutex.lock();
pLS->m_J += BigJ;
pLS->m_dSSD += SSD;
pLS->m_dError += Error;
pLS->m_nErrorPts += ErrorPts;
pLS->m_Mutex.unlock();
// ---------- end contention zone
}
std::vector<int> PlaneFitter::ransacFit(std::vector<Eigen::Vector3d> points3d, std::vector<int> inputIndices){
Eigen::Matrix<double,4,Eigen::Dynamic> D(4,points3d.size());
this->vecToEigenMat(points3d,D);
int N = D.cols();
//cout << N << endl;
// RANSAC variables
float p = 0.99;
float testRansiter = 0;
int bestInlNum = 0;
int best_it=-1;
int inl_num;
srand (time(NULL));
Eigen::Vector4d *testplane = new Eigen::Vector4d();
//keep the indices of the final inliers
std::vector<int> bestInlierIds;
// RANSAC loop
for (int iter=0;iter<2000;iter++){
//sample 3 different random points
std::vector<int> samplevec;
Eigen::Matrix<double,3,4> D_ransac_sample;
int x;
for (int s=0; s<3; s++){
do{
x = rand() % N;
}while (std::find(samplevec.begin(), samplevec.end(), x) != samplevec.end());
samplevec.push_back(x);
D_ransac_sample.block<1,4>(s,0) = D.block<4,1>(0,x).transpose();//<sizeRows,sizeCols>(beginRow,beginCol)
}
//fit model
this->fitPlane(D_ransac_sample,testplane);
std::vector<int> inl_id;
//inlier test
inl_num = 0;
for (int i=0;i<N;i++){
if ( abs(D.block<4,1>(0,i).dot(*testplane)/testplane->head(3).norm()) < this->RANSAC_THRESH ){
inl_num++;
inl_id.push_back(i);
}
}
//cout << "iter,inliers: " << iter <<","<<inl_num<< endl;
//test (adaptive ransac)
testRansiter = log(1-p)/log(1-pow((float)inl_num/N,3));
if (inl_num > bestInlNum){
bestInlNum = inl_num;
best_it = iter;
bestInlierIds.swap(inl_id);
}
}
//bestplane contains the fitted plane
cout << "best iter,inliers: " << best_it <<","<<bestInlNum<< endl;
//Refine best plane
delete this->Dinliers;
delete this->fittedplane;
this->numberOfInliers = bestInlNum;
this->Dinliers = new Eigen::Matrix<double,4,Eigen::Dynamic>(4,bestInlNum);
this->vecToEigenMat(points3d,*this->Dinliers,bestInlierIds);
//get the indices of the inlier points for the output
vector<int> outputIndices;
for (size_t i=0; i< bestInlierIds.size(); i++){
outputIndices.push_back(inputIndices[bestInlierIds[i]]);
}
this->fitPlane(Dinliers->transpose(),testplane);
this->fittedplane = new Eigen::Vector4d();
this->fittedplane = testplane;
return outputIndices;
}
