42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpHomogeneousMatrix.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpQuaternionVector.h>
102 buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
151 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
191 if (list.size() == 12) {
192 std::copy(list.begin(), list.end(),
data);
198 else if (list.size() == 16) {
199 std::copy(list.begin(), list.end(),
data);
200 for (
size_t i = 12; i < 15; ++i) {
201 if (std::fabs(
data[i]) > std::numeric_limits<double>::epsilon()) {
203 "Cannot initialize homogeneous matrix. "
204 "List element %d (%f) should be 0.",
208 if (std::fabs(
data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
210 "Cannot initialize homogeneous matrix. "
211 "List element 15 (%f) should be 1.",
217 "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
240 "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
401 if ((v.size() != 12) && (v.size() != 16)) {
405 for (
unsigned int i = 0; i < 12; ++i) {
406 this->
data[i] =
static_cast<double>(v[i]);
452 if ((v.size() != 12) && (v.size() != 16)) {
456 for (
unsigned int i = 0; i < 12; ++i) {
457 this->
data[i] = v[i];
468 for (
int i = 0; i < 4; ++i) {
469 for (
int j = 0; j < 4; ++j) {
535 (*this) = (*this) * M;
550 "Cannot multiply a (4x4) homogeneous matrix by a "
551 "(%dx1) column vector",
558 for (
unsigned int j = 0; j < 4; ++j) {
559 for (
unsigned int i = 0; i < 4; ++i) {
589 v1[0] = ((*this)[0][0] * v[0]) + ((*
this)[0][1] * v[1]) + ((*
this)[0][2] * v[2]) + ((*
this)[0][3] * v[3]);
590 v1[1] = ((*this)[1][0] * v[0]) + ((*
this)[1][1] * v[1]) + ((*
this)[1][2] * v[2]) + ((*
this)[1][3] * v[3]);
591 v1[2] = ((*this)[2][0] * v[0]) + ((*
this)[2][1] * v[1]) + ((*
this)[2][2] * v[2]) + ((*
this)[2][3] * v[3]);
592 v1[3] = ((*this)[3][0] * v[0]) + ((*
this)[3][1] * v[1]) + ((*
this)[3][2] * v[2]) + ((*
this)[3][3] * v[3]);
623 t_out[0] = (((*this)[0][0] *
t[0]) + ((*
this)[0][1] *
t[1]) + ((*
this)[0][2] *
t[2])) + (*this)[0][3];
624 t_out[1] = (((*this)[1][0] *
t[0]) + ((*
this)[1][1] *
t[1]) + ((*
this)[1][2] *
t[2])) + (*this)[1][3];
625 t_out[2] = (((*this)[2][0] *
t[0]) + ((*
this)[2][1] *
t[1]) + ((*
this)[2][2] *
t[2])) + (*this)[2][3];
646 return (
vpHomogeneousMatrix((*this).getTranslationVector(), (*this).getRotationMatrix() * R));
745 "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
766 const double epsilon = std::numeric_limits<double>::epsilon();
777 unsigned int l_size =
size();
778 for (
unsigned int i = 0; i < l_size; ++i) {
792 for (
unsigned int i = 0; i < 3; ++i) {
793 for (
unsigned int j = 0; j < 3; ++j) {
794 R[i][j] = (*this)[i][j];
804 t[0] = (*this)[0][3];
805 t[1] = (*this)[1][3];
806 t[2] = (*this)[2][3];
833 for (
unsigned int i = 0; i < 3; ++i) {
834 for (
unsigned int j = 0; j < 3; ++j) {
835 (*this)[i][j] = R[i][j];
857 (*this)[0][3] =
t[0];
858 (*this)[1][3] =
t[1];
859 (*this)[2][3] =
t[2];
955 f.open(filename.c_str());
963 for (
unsigned int i = 0; i < 4; ++i) {
964 for (
unsigned int j = 0; j < 4; ++j) {
977 f.open(filename.c_str());
1015 for (
unsigned int i = 0; i < 12; ++i) {
1016 M[i] =
static_cast<float>(this->
data[i]);
1027 for (
unsigned int i = 0; i < 12; ++i) {
1028 M[i] = this->
data[i];
1096 unsigned int nb_rows =
getRows();
1098 for (
unsigned int i = 0; i < nb_rows; ++i) {
1099 c[i] = (*this)[i][j];
1112 const double N =
static_cast<double>(p.size());
1116 size_t p_size = p.size();
1117 for (
size_t i = 0; i < p_size; ++i) {
1118 for (
unsigned int j = 0; j < 3; ++j) {
1119 p_bar[j] += p.at(i).oP[j];
1120 q_bar[j] += q.at(i).oP[j];
1124 for (
unsigned int j = 0; j < 3; ++j) {
1129 vpMatrix pc(
static_cast<unsigned int>(p.size()), 3);
1130 vpMatrix qc(
static_cast<unsigned int>(q.size()), 3);
1132 for (
unsigned int i = 0; i < static_cast<unsigned int>(p_size); ++i) {
1133 for (
unsigned int j = 0; j < 3; ++j) {
1134 pc[i][j] = p.at(i).oP[j] - p_bar[j];
1135 qc[i][j] = q.at(i).oP[j] - q_bar[j];
1173 size_t vec_m_size = vec_M.
size();
1174 for (
size_t i = 0; i < vec_m_size; ++i) {
1175 R = vec_M[i].getRotationMatrix();
1179 meanR /=
static_cast<double>(vec_M.size());
1180 meanT /=
static_cast<double>(vec_M.size());
1186 double det = sv[0] * sv[1] * sv[2];
1196 meanR = U * D * V.
t();
1206 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1218 #ifdef VISP_HAVE_NLOHMANN_JSON
1220 #include <visp3/core/vpJsonParsing.h>
1221 void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j)
const
1228 void vpHomogeneousMatrix::parse_json(
const nlohmann::json &j)
1231 if (j.is_object() && j.contains(
"type")) {
1232 const bool converted = convertFromTypeAndBuildFrom<vpHomogeneousMatrix, vpPoseVector>(j, *
this);
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
double * data
Address of the first element of the data array.
double ** rowPtrs
Address of the first element of each rows.
unsigned int rowNum
Number of rows in the array.
unsigned int size() const
Return the number of elements of the 2D array.
vpArray2D< double > t() const
Compute the transpose of the array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
void load(std::ifstream &f)
friend void from_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
vp_deprecated void setIdentity()
static const std::string jsonTypeName
void print() const
Print the matrix as a pose vector .
vpRotationMatrix getRotationMatrix() const
bool isAnHomogeneousMatrix(double threshold=1e-6) const
void orthogonalizeRotation()
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
vpHomogeneousMatrix & operator*=(const vpHomogeneousMatrix &M)
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void convert(std::vector< float > &M)
friend void to_json(nlohmann::json &j, const vpHomogeneousMatrix &cam)
void extract(vpRotationMatrix &R) const
vpColVector getCol(unsigned int j) const
vpHomogeneousMatrix & operator<<(double val)
void insert(const vpRotationMatrix &R)
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
void save(std::ofstream &f) const
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
vpHomogeneousMatrix & operator,(double val)
static bool isNaN(double value)
static bool equal(double x, double y, double threshold=0.001)
static bool nul(double x, double threshold=0.001)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_W(double cW)
Set the point cW coordinate in the camera frame.
void set_oW(double oW)
Set the point oW coordinate in the object frame.
double get_Y() const
Get the point cY coordinate in the camera frame.
void set_oY(double oY)
Set the point oY coordinate in the object frame.
void set_X(double cX)
Set the point cX coordinate in the camera frame.
double get_W() const
Get the point cW coordinate in the camera frame.
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
double get_Z() const
Get the point cZ coordinate in the camera frame.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
double get_X() const
Get the point cX coordinate in the camera frame.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.