// RQ decomposition HZ A4.1.1 pag.579 void KRt_From_P( const Matx34d &_P, Matx33d &_K, Matx33d &_R, Vec3d &_t ) { libmv::Mat34 P; libmv::Mat3 K, R; libmv::Vec3 t; cv2eigen( _P, P ); libmv::KRt_From_P( P, &K, &R, &t ); eigen2cv( K, _K ); eigen2cv( R, _R ); eigen2cv( t, _t ); }
void fundamentalFromCorrespondences8PointRobust( const Mat_<double> &_x1, const Mat_<double> &_x2, double max_error, Matx33d &_F, vector<int> &_inliers, double outliers_probability ) { libmv::Mat x1, x2; libmv::Mat3 F; libmv::vector<int> inliers; cv2eigen( _x1, x1 ); cv2eigen( _x2, x2 ); libmv::FundamentalFromCorrespondences8PointRobust( x1, x2, max_error, &F, &inliers, outliers_probability ); eigen2cv( F, _F ); // transform from libmv::vector to std::vector int n = inliers.size(); _inliers.resize(n); for( int i=0; i < n; ++i ) { _inliers[i] = inliers.at(i); } }
inline cv::Mat cvtIsometry_egn2ocv(const Eigen::Isometry3d& egn_o) { Eigen::Matrix3d eigenRotation = egn_o.rotation(); Eigen::Matrix<double,3,1> eigenTranslation = egn_o.translation(); cv::Mat R, t; eigen2cv(eigenRotation, R); eigen2cv(eigenTranslation, t); t = t.reshape(1,3); cv::Mat ocv_o = cv::Mat::eye(4,4,CV_64FC1); R.copyTo(ocv_o(cv::Rect(0,0,3,3))); t.copyTo(ocv_o(cv::Rect(3,0,1,3))); return ocv_o; }
void Eigentransformation::createEigenSpace(vector<Mat> &trainingSet, Mat &p, Mat &avg, Mat &vecs, Mat &valsDiag, Mat &eigenSpace){ Size imSize = trainingSet[0].size(); avg = Mat::zeros(imSize, CV_32F); Mat temp = Mat::zeros(imSize, CV_32F); int n = trainingSet.size(); for(int i=0; i<n ; i++){ trainingSet[i].convertTo(temp, CV_32F); avg += temp; } avg = avg/n; p = Mat::zeros(imSize.area(), n, CV_32F); for(int i=0; i<n; i++){ trainingSet[i].convertTo(temp, CV_32F); p.col(i) = temp.reshape(1, imSize.area()) - avg.reshape(1, imSize.area()); } Mat ptp = (p.t())*p; Mat vals; MatrixXf e_M; cv2eigen(ptp, e_M); EigenSolver<MatrixXf> es(e_M); eigen2cv(MatrixXf(es.eigenvectors().real()), vecs); eigen2cv(MatrixXf(es.eigenvalues().real()), vals); valsDiag = Mat::zeros(vecs.size(), CV_32F); float aux; for(int i=0; i<n; i++){ aux = 1/sqrt(vals.at<float>(i)); if(aux!=aux) valsDiag.at<float>(i,i) = 0; else valsDiag.at<float>(i,i) = aux; } eigenSpace = p*vecs*valsDiag; }
virtual void getCameras(OutputArray Rs, OutputArray Ts) { const size_t n_views = libmv_reconstruction_.reconstruction.AllCameras().size(); Rs.create(n_views, 1, CV_64F); Ts.create(n_views, 1, CV_64F); Matx33d R; Vec3d t; for(size_t i = 0; i < n_views; ++i) { eigen2cv(libmv_reconstruction_.reconstruction.AllCameras()[i].R, R); eigen2cv(libmv_reconstruction_.reconstruction.AllCameras()[i].t, t); Mat(R).copyTo(Rs.getMatRef(i)); Mat(t).copyTo(Ts.getMatRef(i)); } }
void MotionModel::PredictNextCameraPose(cv::Point3d& predictedPosition, cv::Vec4d& predictedOrientation) const { // Compute predicted position by integrating linear velocity predictedPosition = position_ + cv::Point3d( linearVelocity_ ); // Compute predicted orientation by integrating angular velocity Eigen::Quaterniond predictedOrientation_e = orientation_ * angularVelocity_; predictedOrientation_e.normalize(); predictedOrientation = eigen2cv( predictedOrientation_e ); }
void isotropicPreconditionerFromPoints( const Mat_<T> &_points, Mat_<T> _T ) { libmv::Mat points; libmv::Mat3 Tr; cv2eigen( _points, points ); libmv::IsotropicPreconditionerFromPoints( points, &Tr ); eigen2cv( Tr, _T ); }
void applyTransformationToPoints( const Mat_<T> &_points, const Mat_<T> &_T, Mat_<T> _transformed_points ) { libmv::Mat points, transformed_points; libmv::Mat3 Tr; cv2eigen( _points, points ); cv2eigen( _T, Tr ); libmv::ApplyTransformationToPoints( points, Tr, &transformed_points ); eigen2cv( transformed_points, _transformed_points ); }
void MotionModel::CurrentCameraPose(cv::Point3d& currentPosition, cv::Vec4d& currentOrientation) const { currentPosition = position_; currentOrientation = eigen2cv( orientation_ ); }
void StereoFrame::CreatePoints( const RowMatcher& matcher, const std::vector<cv::KeyPoint>& keypointsLeft, const cv::Mat& allDescriptorsLeft, const std::vector<cv::KeyPoint>& keypointsRight, const cv::Mat& allDescriptorsRight, std::aligned_vector<MapPoint>& points, std::list<std::pair<size_t, size_t>>& matches ) { #if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE sptam::Timer t_match, t_triangulate; t_match.start(); #endif // per-row matcher for stereo rectify images std::vector<cv::DMatch> cvMatches; matcher.match(keypointsLeft, allDescriptorsLeft, keypointsRight, allDescriptorsRight, cvMatches); #if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE t_match.stop(); #endif // Return if no matches were found if ( cvMatches.empty() ) { std::cerr << "No matches found for triangulation" << std::endl; return; } const Camera& cameraLeft = frameLeft_.GetCamera(); const Camera& cameraRight = frameRight_.GetCamera(); // Compute Projections matrices cv::Matx34d projectionLeft = eigen2cv( cameraLeft.GetProjection() ); cv::Matx34d projectionRight = eigen2cv( cameraRight.GetProjection() ); const std::vector<cv::DMatch>& goodMatches = cvMatches; //std::cout << "F: Correct (by F) / Total matches: " << goodMatches.size() << " / " << cvMatches.size() << std::endl; std::vector<cv::Point2d> matchedPointsLeft; std::vector<cv::Point2d> matchedPointsRight; matchedPointsLeft.reserve( goodMatches.size() ); matchedPointsRight.reserve( goodMatches.size() ); // initialize the points for ( auto match : goodMatches ) { matchedPointsLeft.push_back( keypointsLeft[ match.queryIdx ].pt ); matchedPointsRight.push_back( keypointsRight[ match.trainIdx ].pt ); } #if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE t_triangulate.start(); #endif // Triangulate 3D points from both cameras cv::Mat point3DHomos; cv::triangulatePoints( projectionLeft, projectionRight, matchedPointsLeft, matchedPointsRight, point3DHomos ); #if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE t_triangulate.stop(); #endif // Filter some more by viewing frustum and fill the return parameters for(int i = 0; i < point3DHomos.cols; ++i) { Eigen::Vector3d point = cv2eigen( toInHomo(cv::Vec4d( point3DHomos.col(i) )) ); // if the point is not in range of any camera it is discarded if( not cameraLeft.CanView( point ) || not cameraRight.CanView( point ) ) continue; // this indexes are for the original unmatched collections size_t idxLeft = goodMatches[i].queryIdx; size_t idxRight = goodMatches[i].trainIdx; Eigen::Vector3d normal = point - frameLeft_.GetPosition(); normal.normalize(); points.push_back( MapPoint( point, normal, allDescriptorsLeft.row( idxLeft ), INITIAL_POINT_COVARIANCE ) ); matches.push_back( std::pair<size_t, size_t>(idxLeft, idxRight) ); } #if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE WriteToLog(" xx triangulate_points-create-match ", t_match); WriteToLog(" xx triangulate_points-create-triangulate ", t_triangulate); #endif }
virtual cv::Mat getIntrinsics() const { Mat K; eigen2cv(libmv_reconstruction_.intrinsics->K(), K); return K; }