//--------------------------------------------------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); }