/* * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * */ #include "precomp.hpp" // Eigen #include <Eigen/Core> // OpenCV #include <opencv2/core/eigen.hpp> #include <opencv2/sfm/projection.hpp> #include <opencv2/sfm/triangulation.hpp> #include <opencv2/sfm/fundamental.hpp> #include <opencv2/sfm/numeric.hpp> #include <opencv2/sfm/conditioning.hpp> // libmv headers #include "libmv/multiview/fundamental.h" #include <iostream> using namespace std; namespace cv { namespace sfm { template<typename T> void projectionsFromFundamental( const Mat_<T> &F, Mat_<T> P1, Mat_<T> P2 ) { P1 << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0; Vec<T,3> e2; cv::SVD::solveZ(F.t(), e2); Mat_<T> P2cols = skew(e2) * F; for(char j=0;j<3;++j) { for(char i=0;i<3;++i) P2(j,i) = P2cols(j,i); P2(j,3) = e2(j); } } void projectionsFromFundamental( InputArray _F, OutputArray _P1, OutputArray _P2 ) { const Mat F = _F.getMat(); const int depth = F.depth(); CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F)); _P1.create(3, 4, depth); _P2.create(3, 4, depth); Mat P1 = _P1.getMat(), P2 = _P2.getMat(); // type if( depth == CV_32F ) { projectionsFromFundamental<float>(F, P1, P2); } else { projectionsFromFundamental<double>(F, P1, P2); } } template<typename T> void fundamentalFromProjections( const Mat_<T> &P1, const Mat_<T> &P2, Mat_<T> F ) { Mat_<T> X[3]; vconcat( P1.row(1), P1.row(2), X[0] ); vconcat( P1.row(2), P1.row(0), X[1] ); vconcat( P1.row(0), P1.row(1), X[2] ); Mat_<T> Y[3]; vconcat( P2.row(1), P2.row(2), Y[0] ); vconcat( P2.row(2), P2.row(0), Y[1] ); vconcat( P2.row(0), P2.row(1), Y[2] ); Mat_<T> XY; for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) { vconcat(X[j], Y[i], XY); F(i, j) = determinant(XY); } } void fundamentalFromProjections( InputArray _P1, InputArray _P2, OutputArray _F ) { const Mat P1 = _P1.getMat(), P2 = _P2.getMat(); const int depth = P1.depth(); CV_Assert((P1.cols == 4 && P1.rows == 3) && P1.rows == P2.rows && P1.cols == P2.cols); CV_Assert((depth == CV_32F || depth == CV_64F) && depth == P2.depth()); _F.create(3, 3, depth); Mat F = _F.getMat(); // type if( depth == CV_32F ) { fundamentalFromProjections<float>(P1, P2, F); } else { fundamentalFromProjections<double>(P1, P2, F); } } template<typename T> void normalizedEightPointSolver( const Mat_<T> &_x1, const Mat_<T> &_x2, Mat_<T> _F ) { libmv::Mat x1, x2; libmv::Mat3 F; cv2eigen(_x1, x1); cv2eigen(_x2, x2); libmv::NormalizedEightPointSolver(x1, x2, &F); eigen2cv(F, _F); } void normalizedEightPointSolver( InputArray _x1, InputArray _x2, OutputArray _F ) { const Mat x1 = _x1.getMat(), x2 = _x2.getMat(); const int depth = x1.depth(); CV_Assert(x1.dims == 2 && x1.dims == x2.dims && (depth == CV_32F || depth == CV_64F)); _F.create(3, 3, depth); Mat F = _F.getMat(); // type if( depth == CV_32F ) { normalizedEightPointSolver<float>(x1, x2, F); } else { normalizedEightPointSolver<double>(x1, x2, F); } } template<typename T> void relativeCameraMotion( const Mat_<T> &R1, const Mat_<T> &t1, const Mat_<T> &R2, const Mat_<T> &t2, Mat_<T> R, Mat_<T> t ) { R = R2 * R1.t(); t = t2 - R * t1; } void relativeCameraMotion( InputArray _R1, InputArray _t1, InputArray _R2, InputArray _t2, OutputArray _R, OutputArray _t ) { const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat(); const int depth = R1.depth(); CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size())); CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size())); CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth()); _R.create(3, 3, depth); _t.create(3, 1, depth); Mat R = _R.getMat(), t = _t.getMat(); // type if( depth == CV_32F ) { relativeCameraMotion<float>(R1, t1, R2, t2, R, t); } else { relativeCameraMotion<double>(R1, t1, R2, t2, R, t); } } template<typename T> void motionFromEssential( const Mat_<T> &_E, std::vector<Mat> &_Rs, std::vector<Mat> &_ts ) { libmv::Mat3 E; std::vector < libmv::Mat3 > Rs; std::vector < libmv::Vec3 > ts; cv2eigen(_E, E); libmv::MotionFromEssential(E, &Rs, &ts); _Rs.clear(); _ts.clear(); int n = Rs.size(); CV_Assert(ts.size() == n); for ( int i = 0; i < n; ++i ) { Mat_<T> R_temp, t_temp; eigen2cv(Rs[i], R_temp); _Rs.push_back(R_temp); eigen2cv(ts[i], t_temp); _ts.push_back(t_temp); } } void motionFromEssential( InputArray _E, OutputArrayOfArrays _Rs, OutputArrayOfArrays _ts ) { const Mat E = _E.getMat(); const int depth = E.depth(), cn = 4; CV_Assert(E.cols == 3 && E.rows == 3 && (depth == CV_32F || depth == CV_64F)); _Rs.create(cn, 1, depth); _ts.create(cn, 1, depth); for (int i = 0; i < cn; ++i) { _Rs.create(Size(3,3), depth, i); _ts.create(Size(3,1), depth, i); } std::vector<Mat> Rs, ts; _Rs.getMatVector(Rs); _ts.getMatVector(ts); // type if( depth == CV_32F ) { motionFromEssential<float>(E, Rs, ts); } else { motionFromEssential<double>(E, Rs, ts); } for (int i = 0; i < cn; ++i) { Rs[i].copyTo(_Rs.getMatRef(i)); ts[i].copyTo(_ts.getMatRef(i)); } } template<typename T> int motionFromEssentialChooseSolution( const std::vector<Mat> &Rs, const std::vector<Mat> &ts, const Mat_<T> &K1, const Mat_<T> &x1, const Mat_<T> &K2, const Mat_<T> &x2 ) { Mat_<T> P1, P2, R1 = Mat_<T>::eye(3,3); T val = static_cast<T>(0.0); Vec<T,3> t1(val, val, val); projectionFromKRt(K1, R1, t1, P1); std::vector<Mat_<T> > points2d; points2d.push_back(x1); points2d.push_back(x2); for ( int i = 0; i < 4; ++i ) { const Mat_<T> R2 = Rs[i]; const Vec<T,3> t2 = ts[i]; projectionFromKRt(K2, R2, t2, P2); std::vector<Mat_<T> > Ps; Ps.push_back(P1); Ps.push_back(P2); Vec<T,3> X; triangulatePoints(points2d, Ps, X); T d1 = depth(R1, t1, X); T d2 = depth(R2, t2, X); // Test if point is front to the two cameras. if ( d1 > 0 && d2 > 0 ) { return i; } } return -1; } int motionFromEssentialChooseSolution( InputArrayOfArrays _Rs, InputArrayOfArrays _ts, InputArray _K1, InputArray _x1, InputArray _K2, InputArray _x2 ) { std::vector<Mat> Rs, ts; _Rs.getMatVector(Rs); _ts.getMatVector(ts); const Mat K1 = _K1.getMat(), x1 = _x1.getMat(), K2 = _K2.getMat(), x2 = _x2.getMat(); const int depth = K1.depth(); CV_Assert( Rs.size() == 4 && ts.size() == 4 ); CV_Assert((K1.cols == 3 && K1.rows == 3) && (K1.size() == K2.size())); CV_Assert((x1.cols == 1 && x1.rows == 2) && (x1.size() == x2.size())); CV_Assert((depth == CV_32F || depth == CV_64F) && depth == K2.depth() && depth == x1.depth() && depth == x2.depth()); int solution = 0; // type if( depth == CV_32F ) { solution = motionFromEssentialChooseSolution<float>(Rs, ts, K1, x1, K2, x2); } else { solution = motionFromEssentialChooseSolution<double>(Rs, ts, K1, x1, K2, x2); } return solution; } template<typename T> void fundamentalFromEssential( const Mat_<T> &E, const Mat_<T> &K1, const Mat_<T> &K2, Mat_<T> F ) { F = K2.inv().t() * E * K1.inv(); } void fundamentalFromEssential( InputArray _E, InputArray _K1, InputArray _K2, OutputArray _F ) { const Mat E = _E.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat(); const int depth = E.depth(); CV_Assert(E.cols == 3 && E.rows == 3 && E.size() == _K1.size() && E.size() == _K2.size() && (depth == CV_32F || depth == CV_64F)); _F.create(3, 3, depth); Mat F = _F.getMat(); // type if( depth == CV_32F ) { fundamentalFromEssential<float>(E, K1, K2, F); } else { fundamentalFromEssential<double>(E, K1, K2, F); } } template<typename T> void essentialFromFundamental( const Mat_<T> &F, const Mat_<T> &K1, const Mat_<T> &K2, Mat_<T> E ) { E = K2.t() * F * K1; } void essentialFromFundamental( InputArray _F, InputArray _K1, InputArray _K2, OutputArray _E ) { const Mat F = _F.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat(); const int depth = F.depth(); CV_Assert(F.cols == 3 && F.rows == 3 && F.size() == _K1.size() && F.size() == _K2.size() && (depth == CV_32F || depth == CV_64F)); _E.create(3, 3, depth); Mat E = _E.getMat(); // type if( depth == CV_32F ) { essentialFromFundamental<float>(F, K1, K2, E); } else { essentialFromFundamental<double>(F, K1, K2, E); } } template<typename T> void essentialFromRt( const Mat_<T> &_R1, const Mat_<T> &_t1, const Mat_<T> &_R2, const Mat_<T> &_t2, Mat_<T> _E ) { libmv::Mat3 E; libmv::Mat3 R1, R2; libmv::Vec3 t1, t2; cv2eigen( _R1, R1 ); cv2eigen( _t1, t1 ); cv2eigen( _R2, R2 ); cv2eigen( _t2, t2 ); libmv::EssentialFromRt( R1, t1, R2, t2, &E ); eigen2cv( E, _E ); } void essentialFromRt( InputArray _R1, InputArray _t1, InputArray _R2, InputArray _t2, OutputArray _E ) { const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat(); const int depth = R1.depth(); CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size())); CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size())); CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth()); _E.create(3, 3, depth); Mat E = _E.getMat(); // type if( depth == CV_32F ) { essentialFromRt<float>(R1, t1, R2, t2, E); } else { essentialFromRt<double>(R1, t1, R2, t2, E); } } template<typename T> void normalizeFundamental( const Mat_<T> &F, Mat_<T> F_normalized ) { F_normalized = F * (1.0/norm(F,NORM_L2)); // Frobenius Norm if ( F_normalized(2,2) < 0 ) { F_normalized *= -1; } } void normalizeFundamental( InputArray _F, OutputArray _F_normalized ) { const Mat F = _F.getMat(); const int depth = F.depth(); CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F)); _F_normalized.create(3, 3, depth); Mat F_normalized = _F_normalized.getMat(); // type if( depth == CV_32F ) { normalizeFundamental<float>(F, F_normalized); } else { normalizeFundamental<double>(F, F_normalized); } } template<typename T> void computeOrientation( const Mat_<T> &x1, const Mat_<T> &x2, Mat_<T> R, Mat_<T> t, T s ) { Mat_<T> rr, rl, rt, lt; normalizePoints(x1, rr, rt); normalizePoints(x2, rl, lt); Mat_<T> rrBar, rlBar, rVar, lVar; meanAndVarianceAlongRows(rr, rrBar, rVar); meanAndVarianceAlongRows(rl, rlBar, lVar); Mat_<T> rrp, rlp; rrp = rr - repeat(rrBar, x1.rows, x1.cols); rlp = rl - repeat(rlBar, x2.rows, x2.cols); // TODO: finish implementation // https://github.com/vrabaud/sfm_toolbox/blob/master/sfm/computeOrientation.m#L44 } void computeOrientation( InputArrayOfArrays _x1, InputArrayOfArrays _x2, OutputArray _R, OutputArray _t, double s ) { const Mat x1 = _x1.getMat(), x2 = _x2.getMat(); const int depth = x1.depth(); CV_Assert(x1.size() == x2.size() && (depth == CV_32F || depth == CV_64F)); _R.create(3, 3, depth); _t.create(3, 1, depth); Mat R = _R.getMat(), t = _t.getMat(); // type if( depth == CV_32F ) { computeOrientation<float>(x1, x2, R, t, s); } else { computeOrientation<double>(x1, x2, R, t, s); } } } /* namespace sfm */ } /* namespace cv */