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>
1225 void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j)
const
1232 void 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.
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.