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)
193 if (list.size() == 12) {
194 std::copy(list.begin(), list.end(),
data);
200 else if (list.size() == 16) {
201 std::copy(list.begin(), list.end(),
data);
202 for (
size_t i = 12; i < 15; i++) {
203 if (std::fabs(
data[i]) > std::numeric_limits<double>::epsilon()) {
205 "Cannot initialize homogeneous matrix. "
206 "List element %d (%f) should be 0.",
210 if (std::fabs(
data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
212 "Cannot initialize homogeneous matrix. "
213 "List element 15 (%f) should be 1.",
219 "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
242 "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
403 if (v.size() != 12 && v.size() != 16) {
407 for (
unsigned int i = 0; i < 12; i++)
408 this->
data[i] = (
double)v[i];
453 if (v.size() != 12 && v.size() != 16) {
457 for (
unsigned int i = 0; i < 12; i++)
458 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 for (
unsigned int i = 0; i <
size(); i++) {
791 for (
unsigned int i = 0; i < 3; i++)
792 for (
unsigned int j = 0; j < 3; j++)
793 R[i][j] = (*
this)[i][j];
801 t[0] = (*this)[0][3];
802 t[1] = (*this)[1][3];
803 t[2] = (*this)[2][3];
830 for (
unsigned int i = 0; i < 3; i++)
831 for (
unsigned int j = 0; j < 3; j++)
832 (*
this)[i][j] = R[i][j];
852 (*this)[0][3] =
t[0];
853 (*this)[1][3] =
t[1];
854 (*this)[2][3] =
t[2];
907 (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
908 (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
909 (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
910 (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
982 for (
unsigned int i = 0; i < 4; i++) {
983 for (
unsigned int j = 0; j < 4; j++) {
1026 for (
unsigned int i = 0; i < 12; i++)
1027 M[i] = (
float)(this->
data[i]);
1037 for (
unsigned int i = 0; i < 12; i++)
1038 M[i] = this->
data[i];
1104 unsigned int nb_rows =
getRows();
1106 for (
unsigned int i = 0; i < nb_rows; i++)
1107 c[i] = (*
this)[i][j];
1119 const double N =
static_cast<double>(p.size());
1123 for (
size_t i = 0; i < p.size(); i++) {
1124 for (
unsigned int j = 0; j < 3; j++) {
1125 p_bar[j] += p.at(i).oP[j];
1126 q_bar[j] += q.at(i).oP[j];
1130 for (
unsigned int j = 0; j < 3; j++) {
1135 vpMatrix pc(
static_cast<unsigned int>(p.size()), 3);
1136 vpMatrix qc(
static_cast<unsigned int>(q.size()), 3);
1138 for (
unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
1139 for (
unsigned int j = 0; j < 3; j++) {
1140 pc[i][j] = p.at(i).oP[j] - p_bar[j];
1141 qc[i][j] = q.at(i).oP[j] - q_bar[j];
1179 for (
size_t i = 0; i < vec_M.size(); i++) {
1180 R = vec_M[i].getRotationMatrix();
1184 meanR /=
static_cast<double>(vec_M.size());
1185 meanT /=
static_cast<double>(vec_M.size());
1191 double det = sv[0] * sv[1] * sv[2];
1198 D[0][0] = D[1][1] = 1.0;
1200 meanR = U * D * V.
t();
1210#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1222#ifdef VISP_HAVE_NLOHMANN_JSON
1224#include <visp3/core/vpJsonParsing.h>
1225void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j)
const
1232void vpHomogeneousMatrix::parse_json(
const nlohmann::json &j)
1235 if (j.is_object() && j.contains(
"type")) {
1236 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.
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
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
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
void resize(unsigned int i, bool flagNullify=true)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.