You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
173 lines
4.0 KiB
173 lines
4.0 KiB
/* |
|
* 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/numeric.hpp> |
|
|
|
// libmv headers |
|
#include "libmv/numeric/numeric.h" |
|
|
|
#include <iostream> |
|
|
|
namespace cv |
|
{ |
|
namespace sfm |
|
{ |
|
|
|
template<typename T> |
|
void |
|
meanAndVarianceAlongRows( const Mat_<T> &A, |
|
Mat_<T> mean, |
|
Mat_<T> variance ) |
|
{ |
|
const int n = A.rows, m = A.cols; |
|
|
|
for( int i = 0; i < n; ++i ) |
|
{ |
|
mean(i) = 0; |
|
variance(i) = 0; |
|
|
|
for( int j = 0; j < m; ++j ) |
|
{ |
|
T x = A(i,j); |
|
mean(i) += x; |
|
variance(i) += x*x; |
|
} |
|
} |
|
|
|
mean /= m; |
|
for (int i = 0; i < n; ++i) { |
|
variance(i) = variance(i) / m - (mean(i)*mean(i)); |
|
} |
|
} |
|
|
|
void |
|
meanAndVarianceAlongRows( InputArray _A, |
|
OutputArray _mean, |
|
OutputArray _variance ) |
|
{ |
|
const Mat A = _A.getMat(); |
|
const int depth = A.depth(); |
|
CV_Assert( depth == CV_32F || depth == CV_64F ); |
|
|
|
_mean.create(A.rows, 1, depth); |
|
_variance.create(A.rows, 1, depth); |
|
|
|
Mat mean = _mean.getMat(), variance = _variance.getMat(); |
|
|
|
if( depth == CV_32F ) |
|
{ |
|
meanAndVarianceAlongRows<float>( A, mean, variance ); |
|
} |
|
else |
|
{ |
|
meanAndVarianceAlongRows<double>( A, mean, variance ); |
|
} |
|
} |
|
|
|
|
|
//template<typename T> |
|
//inline Mat |
|
//skewMatMinimal( const Mat_<T> &x ) |
|
//{ |
|
// Mat_<T> skew(2,3); |
|
// skew << 0, -1, x(1), |
|
// 1, 0, -x(0); |
|
// return skew; |
|
//} |
|
// |
|
//Mat |
|
//skewMatMinimal( InputArray _x ) |
|
//{ |
|
// Mat x = _x.getMat(); |
|
// CV_Assert( x.rows == 3 && x.cols == 1 ); |
|
// |
|
// int depth = x.depth(); |
|
// if( depth == CV_32F ) |
|
// { |
|
// return skewMatMinimal<float>(x); |
|
// } |
|
// else |
|
// { |
|
// return skewMatMinimal<double>(x); |
|
// } |
|
//} |
|
|
|
template<typename T> |
|
Mat |
|
skewMat( const Mat_<T> &x ) |
|
{ |
|
Mat_<T> skew(3,3); |
|
skew << 0 , -x(2), x(1), |
|
x(2), 0 , -x(0), |
|
-x(1), x(0), 0; |
|
|
|
return CV_CXX_MOVE(skew); |
|
} |
|
|
|
Mat |
|
skew( InputArray _x ) |
|
{ |
|
const Mat x = _x.getMat(); |
|
const int depth = x.depth(); |
|
CV_Assert( x.size() == Size(3,1) || x.size() == Size(1,3) ); |
|
CV_Assert( depth == CV_32F || depth == CV_64F ); |
|
|
|
Mat skewMatrix; |
|
if( depth == CV_32F ) |
|
{ |
|
skewMatrix = skewMat<float>(x); |
|
} |
|
else if( depth == CV_64F ) |
|
{ |
|
skewMatrix = skewMat<double>(x); |
|
} |
|
else |
|
{ |
|
//CV_Error(CV_StsBadArg, "The DataType must be CV_32F or CV_64F"); |
|
} |
|
|
|
return skewMatrix; |
|
} |
|
|
|
|
|
} /* namespace sfm */ |
|
} /* namespace cv */
|
|
|