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