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); } }
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 calibrate_stereo(const observation_pts_v_t& left_points, const observation_pts_v_t& right_points, const object_pts_v_t& object_points, const cv::Size& image_size, PinholeCameraModel& left, PinholeCameraModel& right) { PinholeCameraModel camera; StereoCameraModel scam; std::vector<cv::Mat> rvecs, tvecs; cv::Mat K_left, K_right, D_right, D_left, R, T, E, F; int flags =cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_ASPECT_RATIO /*| cv::CALIB_ZERO_TANGENT_DIST|cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 |cv::CALIB_FIX_K3*/; cv::Size left_size(image_size), right_size(image_size); double left_rms = cv::calibrateCamera(object_points, left_points, left_size, K_left, D_left, rvecs, tvecs, flags); double right_rms = cv::calibrateCamera(object_points, right_points, right_size, K_right, D_right, rvecs, tvecs, flags); double stereo_rms = cv::stereoCalibrate(object_points, left_points, right_points, K_left, D_left, K_right, D_right, left_size, R, T, E, F, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 500, 1e-6), cv::CALIB_FIX_INTRINSIC | flags); std::cout << "image size:" << image_size.width << ":" << image_size.height << std::endl; std::cout << "R:" << R << std::endl; std::cout << "T:" << T << std::endl; std::cout << "D:" << D_left << std::endl; std::cout << "left rms:" << left_rms << " right rms:" << right_rms << " stereo rms:" << stereo_rms << std::endl; left.setParams(left_size, K_left, D_left, cv::Mat_<double>::eye(3, 3), K_left); right.setParams(right_size, K_right, D_right, cv::Mat_<double>::eye(3, 3), K_right); left.writeCalibration("left.yml"); right.writeCalibration("right.yml"); Eigen::Matrix3d Re; Eigen::Vector3d Te; Eigen::Matrix4d Pe = Eigen::Matrix4d::Identity(); cv2eigen(R, Re); cv2eigen(T.t(), Te); std::cout << Te << std::endl; Pe.block(0, 0, 3, 3) = Re; Pe.block(0, 3, 3, 1) = Te; Pose P; P.transform.matrix() = Pe; scam.setParams(left, right, P); scam.writeCalibration("stereo.yml"); }
// Split the raw image into channels and store in Eigen Matrices. std::vector<Eigen::MatrixXf> imread(const string& path) { cv::Mat mat = cv::imread(path); std::vector<cv::Mat> rgb; cv::split(mat, rgb); std::vector<Eigen::MatrixXf> eigen(rgb.size()); for (int i = 0; i < mat.channels(); ++i) { cv2eigen(rgb[i], eigen[i]); } return eigen; }
// update the camera state given a new camera pose void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation) { // Compute linear velocity cv::Vec3d newLinearVelocity( newPosition - position_ ); // In order to be robust against fast camera movements linear velocity is smoothed over time newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5; // compute rotation between q1 and q2: q2 * qInverse(q1); Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse(); // In order to be robust against fast camera movements angular velocity is smoothed over time newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_); newAngularVelocity.normalize(); // Update the current state variables position_ = newPosition; orientation_ = cv2eigen( newOrientation ); linearVelocity_ = newLinearVelocity; angularVelocity_ = newAngularVelocity; }
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 ); }
// 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 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; }
Isometry3d cvmat2eigen(Mat &r, Mat &t) { //rotation vector to matrix Mat R; Rodrigues(r, R); //opencv matrix to eigen matrix format Matrix3d re; cv2eigen(R, re); AngleAxisd angle(re); //3x3 identity matrix Isometry3d T = Isometry3d::Identity(); T = angle; T(0,3) = t.at<double>(0,0); T(1,3) = t.at<double>(0,1); T(2,3) = t.at<double>(0,2); return T; }
MotionModel::MotionModel(const cv::Point3d& initialPosition, const cv::Vec4d& initialOrientation) : position_( initialPosition ), linearVelocity_(0, 0, 0) { orientation_ = cv2eigen( initialOrientation ); angularVelocity_ = cv2eigen( cv::Vec4d(1, 0, 0, 0) ); }
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 }