Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Added Affine3<T> class

Browse Source
pull/658/head
Anatoly Baksheev 12 years ago
parent 7772811585
commit ceadaed108
2 changed files with 467 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 81
      modules/calib3d/test/test_affine3.cpp
  2. 386
      modules/core/include/opencv2/core/affine.hpp

81
modules/calib3d/test/test_affine3.cpp
Unescape View File

@ -0,0 +1,81 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's 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.
//
// * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/
#include "test_precomp.hpp"
#include "opencv2/core/affine.hpp"
#include "opencv2/calib3d.hpp"
#include <iostream>
TEST(Calib3d_Affine3f, accuracy)
{
cv::Vec3d rvec(0.2, 0.5, 0.3);
cv::Affine3d affine(rvec);
cv::Mat expected;
cv::Rodrigues(rvec, expected);
ASSERT_EQ(0, norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3) != expected));
ASSERT_EQ(0, norm(cv::Mat(affine.linear()) != expected));
cv::Matx33d R = cv::Matx33d::eye();
double angle = 50;
R.val[0] = R.val[4] = std::cos(CV_PI*angle/180.0);
R.val[3] = std::sin(CV_PI*angle/180.0);
R.val[1] = -R.val[3];
cv::Affine3d affine1(cv::Mat(cv::Vec3d(0.2, 0.5, 0.3)).reshape(1, 1), cv::Vec3d(4, 5, 6));
cv::Affine3d affine2(R, cv::Vec3d(1, 1, 0.4));
cv::Affine3d result = affine1.inv() * affine2;
expected = cv::Mat(affine1.matrix.inv(cv::DECOMP_SVD)) * cv::Mat(affine2.matrix, false);
cv::Mat diff;
cv::absdiff(expected, result.matrix, diff);
ASSERT_LT(cv::norm(diff, cv::NORM_INF), 1e-15);
}

386
modules/core/include/opencv2/core/affine.hpp
Unescape View File

@ -0,0 +1,386 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's 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.
//
// * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/
#ifndef __OPENCV_CORE_AFFINE3_HPP__
#define __OPENCV_CORE_AFFINE3_HPP__
#ifdef __cplusplus
#include <opencv2/core.hpp>
namespace cv
{
template<typename T>
class CV_EXPORTS Affine3
{
public:
typedef T float_type;
typedef cv::Matx<float_type, 3, 3> Mat3;
typedef cv::Matx<float_type, 4, 4> Mat4;
typedef cv::Vec<float_type, 3> Vec3;
Affine3();
//Augmented affine matrix
Affine3(const Mat4& affine);
//Rotation matrix
Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
//Rodrigues vector
Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
//Combines all contructors above. Supports 4x4, 3x3, 1x3, 3x1 sizes of data matrix
explicit Affine3(const cv::Mat& data, const Vec3& t = Vec3::all(0));
//Euler angles
Affine3(float_type alpha, float_type beta, float_type gamma, const Vec3& t = Vec3::all(0));
static Affine3 Identity();
//Rotation matrix
void rotation(const Mat3& R);
//Rodrigues vector
void rotation(const Vec3& rvec);
//Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
void rotation(const Mat& data);
//Euler angles
void rotation(float_type alpha, float_type beta, float_type gamma);
void linear(const Mat3& L);
void translation(const Vec3& t);
Mat3 rotation() const;
Mat3 linear() const;
Vec3 translation() const;
Affine3 inv(int method = cv::DECOMP_SVD) const;
// a.rotate(R) is equivalent to Affine(R, 0) * a;
Affine3 rotate(const Mat3& R) const;
// a.translate(t) is equivalent to Affine(E, t) * a;
Affine3 translate(const Vec3& t) const;
// a.concatenate(affine) is equivalent to affine * a;
Affine3 concatenate(const Affine3& affine) const;
template <typename Y> operator Affine3<Y>() const;
Mat4 matrix;
#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
operator Eigen::Transform<T, 3, Eigen::Affine>() const;
#endif
};
template<typename T> Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
template<typename T, typename V> V operator*(const Affine3<T>& affine, const V& vector);
typedef Affine3<float> Affine3f;
typedef Affine3<double> Affine3d;
cv::Vec3f operator*(const cv::Affine3f& affine, const cv::Vec3f& vector);
cv::Vec3d operator*(const cv::Affine3d& affine, const cv::Vec3d& vector);
}
///////////////////////////////////////////////////////////////////////////////////
/// Implementaiton
template<typename T> inline cv::Affine3<T>::Affine3() : matrix(Mat4::eye()) {}
template<typename T> inline cv::Affine3<T>::Affine3(const Mat4& affine) : matrix(affine) {}
template<typename T> inline cv::Affine3<T>::Affine3(const Mat3& R, const Vec3& t)
{
rotation(R);
translation(t);
matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
matrix.val[15] = 1;
}
template<typename T> inline cv::Affine3<T>::Affine3(const Vec3& rvec, const Vec3& t)
{
rotation(rvec);
translation(t);
matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
matrix.val[15] = 1;
}
template<typename T> inline cv::Affine3<T>::Affine3(const cv::Mat& data, const Vec3& t)
{
CV_Assert(data.type() == cv::DataType<T>::type);
if (data.cols == 4 && data.rows == 4)
{
data.copyTo(matrix);
return;
}
rotation(data);
translation(t);
matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
matrix.val[15] = 1;
}
template<typename T> inline cv::Affine3<T>::Affine3(float_type alpha, float_type beta, float_type gamma, const Vec3& t)
{
rotation(alpha, beta, gamma);
translation(t);
matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
matrix.val[15] = 1;
}
template<typename T> inline cv::Affine3<T> cv::Affine3<T>::Identity()
{
return Affine3<T>(cv::Affine3<T>::Mat4::eye());
}
template<typename T> inline void cv::Affine3<T>::rotation(const Mat3& R) { linear(R); }
template<typename T> inline void cv::Affine3<T>::rotation(const Vec3& rvec)
{
double rx = rvec[0], ry = rvec[1], rz = rvec[2];
double theta = std::sqrt(rx*rx + ry*ry + rz*rz);
if (theta < DBL_EPSILON)
rotation(Mat3::eye());
else
{
const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
double c = std::cos(theta);
double s = std::sin(theta);
double c1 = 1. - c;
double itheta = theta ? 1./theta : 0.;
rx *= itheta; ry *= itheta; rz *= itheta;
double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
Mat3 R;
// R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
// where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
for(int k = 0; k < 9; ++k)
R.val[k] = static_cast<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
rotation(R);
}
}
//Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
template<typename T> inline void cv::Affine3<T>::rotation(const cv::Mat& data)
{
CV_Assert(data.type() == cv::DataType<T>::type);
if (data.cols == 3 && data.rows == 3)
{
Mat3 R;
data.copyTo(R);
rotation(R);
}
else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
{
Vec3 rvec;
data.reshape(1, 3).copyTo(rvec);
rotation(rvec);
}
else
CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1");
}
template<typename T> inline void cv::Affine3<T>::rotation(float_type alpha, float_type beta, float_type gamma)
{
rotation(Vec3(alpha, beta, gamma));
}
template<typename T> inline void cv::Affine3<T>::linear(const Mat3& L)
{
matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2];
matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5];
matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8];
}
template<typename T> inline void cv::Affine3<T>::translation(const Vec3& t)
{
matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
}
template<typename T> inline typename cv::Affine3<T>::Mat3 cv::Affine3<T>::rotation() const { return linear(); }
template<typename T> inline typename cv::Affine3<T>::Mat3 cv::Affine3<T>::linear() const
{
cv::Affine3<T>::Mat3 R;
R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2];
R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6];
R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10];
return R;
}
template<typename T> inline typename cv::Affine3<T>::Vec3 cv::Affine3<T>::translation() const
{
return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
}
template<typename T> inline cv::Affine3<T> cv::Affine3<T>::inv(int method) const
{
return matrix.inv(method);
}
template<typename T> inline cv::Affine3<T> cv::Affine3<T>::rotate(const Mat3& R) const
{
Mat3 Lc = linear();
Vec3 tc = translation();
Mat4 result;
result.val[12] = result.val[13] = result.val[14] = 0;
result.val[15] = 1;
for(int j = 0; j < 3; ++j)
{
for(int i = 0; i < 3; ++i)
{
float_type value = 0;
for(int k = 0; k < 3; ++k)
value += R(j, k) * Lc(k, i);
result(j, i) = value;
}
result(j, 3) = R.row(j).dot(tc.t());
}
return result;
}
template<typename T> inline cv::Affine3<T> cv::Affine3<T>::translate(const Vec3& t) const
{
Mat4 m = matrix;
m.val[ 3] += t[0];
m.val[ 7] += t[1];
m.val[11] += t[2];
return m;
}
template<typename T> inline cv::Affine3<T> cv::Affine3<T>::concatenate(const Affine3<T>& affine) const
{
return (*this).rotate(affine.rotation()).translate(affine.translation());
}
template<typename T> template <typename Y> inline cv::Affine3<T>::operator Affine3<Y>() const
{
return Affine3<Y>(matrix);
}
template<typename T> inline cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
{
return affine2.concatenate(affine1);
}
template<typename T, typename V> inline V cv::operator*(const cv::Affine3<T>& affine, const V& v)
{
const typename Affine3<T>::Mat4& m = affine.matrix;
V r;
r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
return r;
}
inline cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
{
const cv::Matx44f& m = affine.matrix;
cv::Vec3f r;
r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
return r;
}
inline cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
{
const cv::Matx44d& m = affine.matrix;
cv::Vec3d r;
r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
return r;
}
#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
template<typename T> inline cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
{
cv::Mat(4, 4, cv::DataType<T>::type, affine.matrix().data()).copyTo(matrix);
}
template<typename T> inline cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
{
Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
cv::Mat(4, 4, cv::DataType<T>::type, a.matrix().data()).copyTo(matrix);
}
template<typename T> inline cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
{
Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
cv::Mat hdr(4, 4, cv::DataType<T>::type, r.matrix().data());
cv::Mat(matrix, false).copyTo(hdr);
return r;
}
template<typename T> inline cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
{
return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
}
#endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
#endif /* __cplusplus */
#endif /* __OPENCV_CORE_AFFINE3_HPP__ */
Write Preview
Loading…
Cancel
Save