//--------------------------------------------------project-----------------------------------------------//
PointXY Camera::project(const Vec3 &coord)
{
PointXY p2D;
cv::Mat p3D(4,1,CV_32FC1);
p3D.at<float>(0) = coord.x_;
p3D.at<float>(1) = coord.y_;
p3D.at<float>(2) = coord.z_;
p3D.at<float>(3) = 1.0;
//cout<<"X: "<< p3D <<endl;
//cout<<"Proj: "<< project_<<endl;
cv::Mat ptmp = project_* p3D;
if(ptmp.at<float>(2)==0)
{
p2D.x = -1;
p2D.y = -1;
}
else
{
p2D.x = ptmp.at<float>(0)/ptmp.at<float>(2);
p2D.y = ptmp.at<float>(1)/ptmp.at<float>(2);
}
// cout<<"p2Dx: "<< p2D.x << " p2Dy: "<< p2D.y<<endl;
ptmp.release();
p3D.release();
return p2D;
}
qglviewer::Vec Camera::project(const qglviewer::Vec& coord)
{
qglviewer::Vec p2D;
cv::Mat p3D(4,1,CV_32FC1);
p3D.at<float>(0) = coord.x;
p3D.at<float>(1) = coord.y;
p3D.at<float>(2) = coord.z;
p3D.at<float>(3) = 1.0;
cv::Mat ptmp = project_* p3D;
if(ptmp.at<float>(2)==0)
{
p2D.x = -1;
p2D.y = -1;
p2D.z = 1;
}
else
{
p2D.x = ptmp.at<float>(0)/ptmp.at<float>(2);
p2D.y = ptmp.at<float>(1)/ptmp.at<float>(2);
p2D.z = 1.0;
}
ptmp.release();
p3D.release();
return p2D;
}
int GraphOptimizer_MRPT::addVertex(Eigen::Matrix4d& vertexPose)
{
//Transform the vertex pose from Eigen::Matrix4d to MRPT CPose3D
mrpt::math::CMatrixDouble44 vertexPoseMRPT;
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
vertexPoseMRPT(i,j)=vertexPose(i,j);
}
}
if(rigidTransformationType==GraphOptimizer::SixDegreesOfFreedom) //Rigid transformation 6DoF
{
mrpt::poses::CPose3D p(vertexPoseMRPT);
//Add the vertex to the graph
graph3D.nodes[vertexIdx] = p; vertexIdx++;
}
else //Rigid transformation 3DoF
{
mrpt::poses::CPose3D p3D(vertexPoseMRPT);
mrpt::poses::CPose2D p(p3D); //Construct a 2D pose from a 3D pose (x,y,phi):=(x',y',yaw')
//Add the vertex to the graph
graph2D.nodes[vertexIdx] = p; vertexIdx++;
}
//Return the vertex index
return vertexIdx-1;
}
void CGraphicsRigidDiaphragmLoad::GetTextPoint( CPoint& p2D, double arrowLength )
{
glMatrixMode( GL_MODELVIEW );
glPushMatrix();
glTranslatef( (float)m_loc.x, (float)m_loc.y, (float)m_loc.z );
glRotatef( float(m_rot*180/M_PI), (float)(m_axis.x), (float)(m_axis.y), (float)(m_axis.z) );
if( isTranslation( m_etDir ) )
{
CPoint3D p3D( -arrowLength*1.1, 0., 0. );
GetScreenPtFrom3DPoint( p3D, p2D );
}
else
{
CPoint3D p3D( 0., arrowLength, 0. );
GetScreenPtFrom3DPoint( p3D, p2D );
}
glPopMatrix();
}
cv::Mat mu::dlt(const std::vector<cv::Mat_<double> > &points3D, const std::vector<cv::Matx31d> &points2D) {
cv::Mat A(2 * points3D.size(), 12, CV_64F);
double *p = A.ptr<double>(0);
//Compute A
for (uint i = 0; i < points3D.size(); ++i) {
// TODO: Put matrix from theory
cv::Matx41d p3D(points3D.at(i).at<double>(0), points3D.at(i).at<double>(1), points3D.at(i).at<double>(2), 1);
cv::Point2d p2D(points2D.at(i).val[0], points2D.at(i).val[1]);
// first row
*p++ = p3D.val[0];
*p++ = p3D.val[1];
*p++ = p3D.val[2];
*p++ = 1;
*p++ = 0;
*p++ = 0;
*p++ = 0;
*p++ = 0;
*p++ = -(p2D.x * p3D.val[0]);
*p++ = -(p2D.x * p3D.val[1]);
*p++ = -(p2D.x * p3D.val[2]);
*p++ = -p2D.x;
// second row
*p++ = 0;
*p++ = 0;
*p++ = 0;
*p++ = 0;
*p++ = p3D.val[0];
*p++ = p3D.val[1];
*p++ = p3D.val[2];
*p++ = 1;
*p++ = -(p2D.y * p3D.val[0]);
*p++ = -(p2D.y * p3D.val[1]);
*p++ = -(p2D.y * p3D.val[2]);
*p++ = -p2D.y;
}
cv::SVD svd(A, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
cv::Mat Hn = svd.vt.row(11).reshape(0, 3); // H12
return Hn;
}
void CGraphicsReaction::Draw( CDC* pDC, const CWindowFilter& filter, double scale, CGLText* text, double length )
{
ASSERT( m_pN && m_pRC );
if( !m_pN || !m_pRC || m_pN->isFree() )
return;
float w = (float)ini.size().reactionWidth;
float l = length;
const CResult* cR = m_pRC->result( *m_pN );
double xi = m_pN->x();
double yi = m_pN->y();
double zi = m_pN->z();
if( cR )
{
xi += scale*cR->displacement( DX );
yi += scale*cR->displacement( DY );
zi += scale*cR->displacement( DZ );
}
COLORREF c = ini.color().nodes;
ApplyAmbientGLMaterialiv( c );
glPushMatrix();
glTranslatef( float( xi ), float( yi ), float( zi ) );
double mv[16];
glGetDoublev( GL_MODELVIEW_MATRIX, mv );
for( int i=0; i<3; i++ ){
for( int j=0; j<3; j++ ) {
if ( i==j )
mv[i*4+j] = 1.0;
else
mv[i*4+j] = 0.0;
}
}
CPoint p2D;
CVector3D mvz( mv[8], mv[9], mv[10] );
CPoint3D p3D( 0., -l, 0. );
if( m_pN->isFixed( DX ) && !zero(cR->force( DX )) && filter.node.fx )
{
CString label = "FX = ";
label += Units::show( FORCE_UNIT, cR->force( DX ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
if( cR->force( DX ) < 0. )
glRotatef( 180.f, 0.f, 0.f, 1.f );
DrawLineArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
}
if( m_pN->isFixed( DY ) && !zero(cR->force( DY )) && filter.node.fy )
{
CString label = "FY = ";
label += Units::show( FORCE_UNIT, cR->force( DY ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
glRotatef( 90.f, 0.f, 0.f, 1.f );
if( cR->force( DY ) < 0. )
glRotatef( 180.f, 0.f, 0.f, 1.f );
DrawLineArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
}
if( m_pN->isFixed( DZ ) && !zero(cR->force( DZ )) && filter.node.fz )
{
CString label = "FZ = ";
label += Units::show( FORCE_UNIT, cR->force( DZ ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
glRotatef( 90.f, 0.f, 1.f, 0.f );
if( cR->force( DZ ) < 0. )
glRotatef( 180.f, 0.f, 0.f, 1.f );
DrawLineArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
}
if( m_pN->isFixed( RX ) && !zero(cR->force( RX )) && filter.node.mx )
{
CString label = "MX = ";
label += Units::show( MOMENT_UNIT, cR->force( RX ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
glRotatef( 90.f, 0.f, 1.f, 0.f );
if( cR->force( RX ) < 0. )
glRotatef( 180.f, 0.f, 0.f, 1.f );
DrawSolidMomentArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
text->AddLine( label );
}
if( m_pN->isFixed( RY ) && !zero(cR->force( RY )) && filter.node.my )
{
CString label = "MY = ";
label += Units::show( MOMENT_UNIT, cR->force( RY ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
glRotatef( 90.f, 1.f, 0.f, 0.f );
if( cR->force( RY ) > 0. )
glRotatef( 180.f, 0.f, 0.f, 1.f );
DrawSolidMomentArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
}
if( m_pN->isFixed( RZ ) && !zero(cR->force( RZ )) && filter.node.mz )
{
CString label = "MZ = ";
label += Units::show( MOMENT_UNIT, cR->force( RZ ));
text->ClearText();
text->AddLine( label );
glPushMatrix();
if( cR->force( RZ ) < 0. )
glRotatef( 180.f, 0.f, 1.f, 0.f );
DrawSolidMomentArrow( l, w, pDC, text,
ini.font().graphWindow.name,
ini.font().graphWindow.size,
ini.color().nodalLoads);
glPopMatrix();
}
glPopMatrix();
}
/**
* @brief Generates a optimised point cloud from a collection of processed keyframes using sparse bundle adjustment.
* @param allFrames A vector containing keyframes which have been completely processed.
* @param outputCloud The cloud generated from the keyframes.
*/
void utils::calculateSBACloud(std::vector<KeyframeContainer>& allFrames,boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >& outputCloud)
{
sba::SysSBA bundleAdjuster;
//set the verbosity of the bundle adjuster
#ifndef NDEBUG
bundleAdjuster.verbose=100;
#else
bundleAdjuster.verbose=0;
#endif
//a list of keypoints that already have been added to the SBA system
std::vector<std::pair<int,uint> > addedPoints; //(frameID,keypointIndex)
//maps frame IDs to frame index in the vector
std::map<int,uint> ID2Ind;
for(uint f=0;f<allFrames.size();++f) ID2Ind[allFrames[f].ID]=f;
//maps the frame vector index to the node vector index
std::map<uint,int> fInd2NInd;
//count the number of 2D/3D-points and camera nodes if in debug mode
#ifndef NDEBUG
uint nrP3D=0;
uint nrP2D=0;
uint nrC=0;
#endif
//add every valid frame as a camera node to the sba system
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//get rotation and translation from the projection matrix
Eigen::Matrix3d rot;
cv::Mat_<double> cvT;
Eigen::Vector4d t;
//get rotation
for(int x=0;x<3;++x) for(int y=0;y<3;++y) rot(x,y)=allFrames[f].projectionMatrix(x,y);
//solve for translation
cv::solve(allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(0,3)),allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(3,4)),cvT);
//save translation
for(int x=0;x<3;++x) t(x)=-cvT(x,0);
t(3)=1.0;
//convert rotation to quaternion
Eigen::Quaterniond qrot(rot);
qrot.normalize();
//convert camera calibration matrix
frame_common::CamParams cameraParameters;
cameraParameters.fx=allFrames[f].cameraCalibration(0,0);
cameraParameters.fy=allFrames[f].cameraCalibration(1,1);
cameraParameters.cx=allFrames[f].cameraCalibration(0,2);
cameraParameters.cy=allFrames[f].cameraCalibration(1,2);
cameraParameters.tx=0.0;
//add the frame as a camera node
fInd2NInd[f]=bundleAdjuster.addNode(t,qrot,cameraParameters,false);
#ifndef NDEBUG
++nrC;
#endif
}
else
fInd2NInd[f]=-1;
}
//add the points to the correct nodes
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//add the points
for(uint m=0;m<allFrames[f].matches.size();++m)
{
std::pair<int,uint> myP(allFrames[f].ID,m);
bool goodToAdd=!utils::vectorContainsElement(addedPoints,myP);
//point hasn't been added yet
if(goodToAdd)
{
//check if the point has valid depth information
float depth=allFrames[f].depthImg.image.at<float>(allFrames[f].keypoints[m].pt.y,allFrames[f].keypoints[m].pt.x);
if(isnan(depth)==0)
{
//find the largest completely connected component emanating from this point (clique)
std::vector<std::pair<int,uint> > pointsComplete;
pointsComplete.push_back(std::pair<int,uint>(allFrames[f].ID,m));
//check all matches of the point if they haven't been added yet and their frame is valid
for(uint o=0;o<allFrames[f].matches[m].size();++o)
{
std::pair<int,uint> newElement(allFrames[f].matches[m][o].first.val[0],allFrames[f].matches[m][o].first.val[1]);
if(!utils::vectorContainsElement(addedPoints,newElement) && !allFrames[ID2Ind[newElement.first]].invalid)
pointsComplete.push_back(newElement);
}
//weed out all points that are not completely connected
while(pointsComplete.size()>1)
{
//find the point with the fewest connections (if the component is completely connected all points have the same number of connections)
uint minCorrespondences=pointsComplete.size();
uint worstInd=0;
for(uint i=0;i<pointsComplete.size();++i)
{
//count the number of connections that this point has
uint fInd=ID2Ind[pointsComplete[i].first];
uint myCorr=0;
for(uint q=0;q<allFrames[fInd].matches.size();++q)
for(uint r=0;r<allFrames[fInd].matches[q].size();++r)
{
std::pair<int,uint> tmpElement(allFrames[fInd].matches[q][r].first.val[0],allFrames[fInd].matches[q][r].first.val[1]);
if(utils::vectorContainsElement(pointsComplete,tmpElement))
++myCorr;
}
//save this point if it is the current worst
if(myCorr<minCorrespondences)
{
minCorrespondences=myCorr;
worstInd=i;
}
}
//if the worst point has not the maximal number of connections erase it, else break as all points have the maximal number of connections
if(minCorrespondences<pointsComplete.size()-1)
{
pointsComplete.erase(pointsComplete.begin()+worstInd);
}
else
break;
}
//now pointsComplete contains the clique if the clique has more than one isolated point, write the points to the system
if(pointsComplete.size()>1)
{
//calculate the 3D point and add it to the system
cv::Mat_<double> p2D(2,1,0.0);
p2D(0,0)=allFrames[f].keypoints[m].pt.x;
p2D(1,0)=allFrames[f].keypoints[m].pt.y;
cv::Mat_<double> p3D=reprojectImagePointTo3D(p2D,allFrames[f].cameraCalibration,allFrames[f].projectionMatrix,depth);
Eigen::Vector4d newPoint;
for(uint x=0;x<4;++x) newPoint(x)=p3D(x,0);
int pointIndex=bundleAdjuster.addPoint(newPoint);
#ifndef NDEBUG
++nrP3D;
#endif
//add all image points corresponding to the 3D point to the system
for(uint i=0;i<pointsComplete.size();++i)
{
uint fInd=ID2Ind[pointsComplete[i].first];
Eigen::Vector2d imgPoint;
imgPoint(0)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.x;
imgPoint(1)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.y;
bundleAdjuster.addMonoProj(fInd2NInd[fInd],pointIndex,imgPoint);
#ifndef NDEBUG
++nrP2D;
#endif
}
}
}
}
}
}
}
#ifndef NDEBUG
ROS_INFO("C: %u;3D: %u;2D: %u",nrC,nrP3D,nrP2D);
#endif
//remove bad tracks
bundleAdjuster.calcCost();
bundleAdjuster.removeBad(2);
bundleAdjuster.reduceTracks();
//run 100 iterations of sba
bundleAdjuster.doSBA(100,1e-3, SBA_SPARSE_CHOLESKY);
//save the new projective matrix calculated with sba
for(uint f=0;f<allFrames.size();++f)
if(!allFrames[f].invalid)
for(int x=0;x<3;++x) for(int y=0;y<4;++y) allFrames[f].projectionMatrix(x,y)=bundleAdjuster.nodes[fInd2NInd[f]].w2n(x,y);
//generate the point cloud from the new projection matrices
utils::generateCloud(allFrames,*outputCloud);
}
