c++ eigen库_modbus指令库

c++ eigen库_modbus指令库测试程序:#include<iostream>#include<Eigen/Eigen>usingnamespacestd;#definepi3.14159265359intmain(){ cout<<“##——————-搞清旋转关系——————-##”<<en…

大家好,又见面了,我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全系列IDE稳定放心使用

查看Eigen版本 

$ head -n 20 /usr/include/eigen3/Eigen/src/Core/util/Macros.h
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 92

版本就是3.2.92


搞清旋转关系

c++ eigen库_modbus指令库

eigen_test.cc: 

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
 
// wrap the angle within [-PI, PI)
double WrapToPI(double ang_in_rad) {
  int c = ang_in_rad / (2.0 * M_PI);
  ang_in_rad -= c * (2.0 * M_PI);
  if (ang_in_rad < -M_PI) {
    ang_in_rad += 2.0 * M_PI;
  }
  if (ang_in_rad >= M_PI) {
    ang_in_rad -= 2.0 * M_PI;
  }
  return ang_in_rad;
}
 
int main(int argc, char *argv[]) {
  double roll = 30;
  double pitch = 45;
  double yaw = 90;
  std::cout << "\n指定欧拉角(roll pitch yaw): " << roll << " " << pitch << " " << yaw << std::endl;
 
  Eigen::Vector3d p1(0, 1, 0); // 点p在o1参考系下的坐标为(0, 1, 0)
  std::cout << "\n点p在o1参考系下的坐标p1: " << p1.transpose() << std::endl;
 
  Eigen::AngleAxisd v_21(yaw * M_PI / 180, Eigen::Vector3d::UnitZ()); // o1参考系绕其z轴(转yaw)顺时针旋转90度得到o2参考系
  Eigen::Matrix3d R_21 = v_21.matrix();
  Eigen::Vector3d p2 = R_21 * p1; // 点p在o2参考系下的坐标为(-1, 0, 0)
  std::cout << "\n点p在o2参考系下的坐标p2: " << p2.transpose() << std::endl;
 
  Eigen::AngleAxisd v_32(pitch * M_PI / 180, Eigen::Vector3d::UnitY()); // o2参考系绕其y轴(pitch)顺时针旋转45度得到o3参考系
  Eigen::Matrix3d R_32 = v_32.matrix();
  Eigen::Vector3d p3 = R_32 * p2; // 点p在o3参考系下的坐标为(-0.707, 0, 0.707)
  std::cout << "\n点p在o3参考系下的坐标p3: " << p3.transpose() << std::endl;
 
  Eigen::AngleAxisd v_43(roll * M_PI / 180, Eigen::Vector3d::UnitX()); // o3参考系绕其x轴(roll)顺时针旋转30度得到o4参考系
  Eigen::Matrix3d R_43 = v_43.matrix();
  Eigen::Vector3d p4 = R_43 * p3; // 点p在o4参考系下的坐标为(-0.707, -0.35, 0.61)
  std::cout << "\n点p在o4参考系下的坐标p4: " << p4.transpose() << std::endl;
 
  Eigen::Matrix3d R_41 = R_43 * R_32 * R_21; // 先绕z轴顺时针旋转90度,再绕y轴顺时针旋转45度,最后绕x轴顺时针旋转30度
  // Eigen::Matrix3d R_41 = v_43 * v_32 * v_21;
  p4 = R_41 * p1; // 点p在o4参考系下的坐标为(-0.707, -0.353, 0.612)
  std::cout << "\n点p在o4参考系下的坐标p4: " << p4.transpose() << std::endl;
 
  // 旋转矩阵->欧拉角
  Eigen::Vector3d euler_angles = R_41.eulerAngles(0, 1, 2); // (0,1,2) 表示分别绕XYZ轴顺序(与上面旋转顺序相反),即按roll,pitch,yaw顺序,顺时针为正
  // Euler's angles are not unique.
  // eigen has two sets of euler angles: (a, b, c) or (pi+a, pi-b, pi+c)
  // In your XYZ convention, both (0, pi, pi) and (pi, 0, 0) represents the same rotation, and both are correct.
  // The Eigen::eulerAngles method consistently chooses to minimize first angles.
  if (std::fabs(euler_angles(1)) > M_PI / 2) {
    euler_angles(0) = WrapToPI(M_PI + euler_angles(0));
    euler_angles(1) = WrapToPI(M_PI - euler_angles(1));
    euler_angles(2) = WrapToPI(M_PI + euler_angles(2));
  }
  std::cout << "\n旋转矩阵->欧拉角(roll pitch yaw): " << euler_angles.transpose() * 180 / M_PI << std::endl; // 30 45 90
 
  return 0;
}

输出:

c++ eigen库_modbus指令库


不同旋转表示及相互转换

eigen_test.cc:  

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>

int main(int argc, char *argv[]) {
  // 单位四元素
  Eigen::Quaterniond q = Eigen::Quaterniond(1, 0, 0, 0); // (w,x,y,z)
  // Eigen::Quaterniond q(1, 0, 0, 0); // (w,x,y,z)
  // Eigen::Quaterniond q(Eigen::Vector4d(0, 0, 0, 1)); // (x,y,z,w)
  std::cout << "\n单位四元素:\n" << q.coeffs() << std::endl; // (x,y,z,w)

  // 单位旋转矩阵
  Eigen::Matrix3d rotation_matrix3d = Eigen::Matrix3d::Identity();
  std::cout << "\n单位旋转矩阵:\n" << rotation_matrix3d << std::endl;

  // 旋转向量(轴角)
  Eigen::AngleAxisd angle_axis(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 绕z轴顺时针旋转45°(yaw)
  std::cout << "\n旋转向量:\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;

  // 欧拉角
  Eigen::Vector3d euler_angles(0, 0, 45); // roll pitch yaw(自定义)
  std::cout << "\n欧拉角:\n(roll pitch yaw) = " << euler_angles.transpose() << std::endl;

  // 旋转向量->旋转矩阵
  rotation_matrix3d = angle_axis.matrix();
  // rotation_matrix3d = angle_axis.toRotationMatrix();
  std::cout << "\n旋转向量->旋转矩阵:\n" << rotation_matrix3d << std::endl;

  // 旋转矩阵->旋转向量(轴角)
  angle_axis.fromRotationMatrix(rotation_matrix3d);
  // angle_axis = rotation_matrix3d;
  std::cout << "\n旋转矩阵->旋转向量(轴角):\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;

  // 旋转向量(轴角)->四元素
  q = Eigen::Quaterniond(angle_axis);
  std::cout << "\n旋转向量(轴角)->四元素:\n(w x y z) = " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;

  // 四元素->旋转向量(轴角)
  angle_axis = q;
  std::cout << "\n四元素->旋转向量(轴角):\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;

  // 旋转矩阵->四元素
  q = Eigen::Quaterniond(rotation_matrix3d);
  // q = rotation_matrix3d;
  std::cout << "\n旋转矩阵->四元素:\n(w x y z) = " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;

  // 四元素->旋转矩阵
  rotation_matrix3d = q.matrix();
  // rotation_matrix3d = q.toRotationMatrix();
  std::cout << "\n四元素->旋转矩阵:\n" << rotation_matrix3d << std::endl;

  // 旋转矩阵->欧拉角
  euler_angles = rotation_matrix3d.eulerAngles(0, 1, 2);
  std::cout << "\n旋转矩阵->欧拉角:\n(roll pitch yaw) = " << euler_angles.transpose() * 180 / M_PI << std::endl;

  return 0;
}

输出: 

c++ eigen库_modbus指令库


基础用法

eigen_test.cc:  

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>

int main(int argc, char *argv[]) {
  // 向量(列向量)
  Eigen::Vector3d v1(0, 0, 0); // 声明并定义
  v1.y() = 1;
  v1[2] = 2;
  std::cout << "v1: " << v1.transpose() << std::endl;

  Eigen::Vector3d v2;
  v2 << 2, 2, 2; // 先声明后定义
  std::cout << "v2: " << v2.transpose() << std::endl;

  Eigen::Vector3d t;
  t.setZero(); // 各分量设为0
  // t = Eigen::Vector3d::Zero();
  std::cout << "t: " << t.transpose() << std::endl;
  t.setOnes(); // 各分量设为1
  // t = Eigen::Vector3d::Ones();
  std::cout << "t: " << t.transpose() << std::endl;

  // 矩阵
  Eigen::Matrix<double,3,4> M;
  M << 1,0,0,1,
       0,2,0,1,
       0,0,1,1;
  M(1,1) = 1;
  std::cout << "M:\n" << M << std::endl;

  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  std::cout << "R:\n" << R << std::endl;

  // 变换矩阵(4x4)
  Eigen::Matrix4d T;
  T << R, t, 0, 0, 0, 1;
  std::cout << "T:\n" << T << std::endl;

  // 数学运算
  v2 = R.inverse()*v2 - t;
  std::cout << "v2: " << v2.transpose() << std::endl;
  std::cout << "v2模长: " << v2.norm() << std::endl;
  std::cout << "v2单位向量: " << v2.normalized().transpose() << std::endl;
  std::cout << "v1点乘v2: " << v1.dot(v2) << std::endl;
  std::cout << "v1叉乘v2: " << v1.cross(v2).transpose() << std::endl; // 叉乘只能用于长度为3的向量

  // 块操作
  R = T.block<3, 3>(0, 0);
  t = T.block<3, 1>(0, 3);
  std::cout << "旋转R:\n" << T.topLeftCorner(3, 3) << std::endl;
  std::cout << "平移t: " << T.topRightCorner(3, 1).transpose() << std::endl;

  // 欧式变换矩阵(Isometry)
  Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); // 虽然称为3d,实质上是4x4的矩阵(旋转R+平移t)

  // 旋转部分赋值
  // T1.linear() = Eigen::Matrix3d::Identity();
  // T1.linear() << 1, 0, 0, 0, 1, 0, 0, 0, 1;
  // T1.rotate(Eigen::Matrix3d::Identity());
  T1.rotate(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0,0,1)));

  // 平移部分赋值
  // T1.pretranslate(Eigen::Vector3d(1, 1, 1));
  T1.translation() = Eigen::Vector3d(1, 1, 1);

  std::cout << "T1:\n" << T1.matrix() << std::endl; // 输出4x4变换矩阵
  std::cout << "R1:\n" << T1.linear().matrix() << std::endl; // 输出旋转部分
  std::cout << "t1:\n" << T1.translation().transpose() << std::endl; // 输出平移部分
	
  Eigen::Quaterniond q(T1.linear());
  std::cout << "q: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;

  Eigen::Isometry3d T2(q);
  T2(0,3) = 1;
  T2(1,3) = 2;
  T2(2,3) = 3;
  std::cout << "T2:\n" << T2.matrix() << std::endl;

  Eigen::Vector3d v3(1,1,0);
  v3 = T1 * v3; // 相当于R1*v1+t1,隐含齐次坐标(1,1,0,1)
  std::cout << "v3: " << v3.transpose() << std::endl;

  // 仿射变换矩阵(Affine3d)
  Eigen::Translation3d t;
  Eigen::Quaterniond q;
  Eigen::Affine3d T = t * q;

  return 0;
}

 输出:

c++ eigen库_modbus指令库


Eigen::MatrixXd B = Eigen::MatrixXd::Identity(6, 5);
Eigen::VectorXd b(5);
b << 1, 4, 6, -2, 0.4;
Eigen::VectorXd Bb = B * b;
std::cout << "The multiplication of B * b is " << std::endl << Bb << std::endl;

Eigen::MatrixXd A(3, 2);
A << 1, 2,
2, 3,
3, 4;
Eigen::MatrixXd B = A.transpose();// the transpose of A is a 2x3 matrix
Eigen::MatrixXd C = (B * A).inverse();// computer the inverse of BA, which is a 2x2 matrix

Eigen::MatrixXd A = Eigen::MatrixXd::Random(7, 9);
Eigen::MatrixXd A = Eigen::MatrixXd::Random(7, 9);
std::cout << "The element at fourth row and 7the column is " << A(3, 6) << std::endl;
Eigen::MatrixXd B = A.block(1, 2, 3, 3);
std::cout << "Take sub-matrix whose upper left corner is A(1, 2)" << std::endl << B << std::endl;
Eigen::VectorXd a = A.col(1); // take the second column of A
Eigen::VectorXd b = B.row(0); // take the first row of B
Eigen::VectorXd c = a.head(3);// take the first three elements of a
Eigen::VectorXd d = b.tail(2);// take the last two elements of b

Eigen::Quaterniond q1(2, 0, 1, -3); 
q1.normalize();
std::cout << "To represent rotation, we need to normalize it such that its length is " << q1.norm() << std::endl;

Eigen::Vector3d v(1, 2, -1);
Eigen::Quaterniond q2;
q2.w() = 0;
q2.vec() = v;
Eigen::Quaterniond q = q1 * q2 * q1.inverse(); 

Eigen::Quaterniond a = Eigen::Quterniond::Identity();

Eigen旋转内插值

eigen_test.cc:

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>

int main() {
  Eigen::AngleAxisd angle_axis1(M_PI / 6, Eigen::Vector3d(0, 0, 1)); // 沿z轴(yaw)顺时针旋转30°
  Eigen::Quaterniond q1 = Eigen::Quaterniond(angle_axis1);
  Eigen::Vector3d t1(3, 3, 3);
  Eigen::AngleAxisd angle_axis2(M_PI / 2, Eigen::Vector3d(0, 0, 1)); // 沿z轴(yaw)顺时针旋转90°
  Eigen::Quaterniond q2 = Eigen::Quaterniond(angle_axis2);
  Eigen::Vector3d t2(9, 9, 9);
  double ratio = 1.0 / 3;

  auto q = q1.slerp(ratio, q2);
  q.normalize();
  const auto &t = (1 - ratio) * t1 + ratio * t2;
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  // Eigen::Matrix4d T{Eigen::Matrix4d::Identity()};
  T.block<3, 3>(0, 0) = q.toRotationMatrix();
  T.block<3, 1>(0, 3) = t;

  Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  std::cout << "yaw pitch roll: " << euler_angles.transpose() * 180 / M_PI << std::endl;
  std::cout << "t: " << t.transpose() << std::endl;

  return 0;
}

输出:

c++ eigen库_modbus指令库


解线性方程组

Eigen提供了解线性方程的计算方法,包括LU分解法,QR分解法,SVD(奇异值分解)、特征值分解等。对于一般形如Ax=b的线性系统,解方程的方式一般是将矩阵A进行分解,当然最基本的方法是高斯消元法。

Eigen内置的解线性方程组的算法如下表所示: 

Decomposition Method Requirements
on the matrix
Speed
(small-to-medium)
Speed
(large)
Accuracy
PartialPivLU partialPivLu() Invertible ++ ++ +
FullPivLU fullPivLu() None – – +++
HouseholderQR householderQr() None ++ ++ +
ColPivHouseholderQR colPivHouseholderQr() None + +++
FullPivHouseholderQR fullPivHouseholderQr() None – – +++
CompleteOrthogonalDecomposition completeOrthogonalDecomposition() None + +++
LLT llt() Positive definite +++ +++ +
LDLT ldlt() Positive or negative
semidefinite
+++ + ++
BDCSVD bdcSvd() None +++
JacobiSVD jacobiSvd() None – – – +++

 eigen_test.cc:  

#include <iostream>
#include <Eigen/Dense>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"

using namespace std;
using namespace Eigen;

int main() {
    // Basic linear solving
    Matrix3f A;
    Vector3f b;
    A << 1,2,3,  4,5,6,  7,8,10;
    b << 3, 3, 4;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the vector b:\n" << b << endl;
    Vector3f x = A.colPivHouseholderQr().solve(b);
    cout << "The solution is:\n" << x << endl;

    Matrix2f A, b;
    LLT<Matrix2f> llt;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(A);
    cout << "The solution is:\n" << llt.solve(b) << endl;
    A(1,1)++;
    cout << "The matrix A is now:\n" << A << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(A);
    cout << "The solution is now:\n" << llt.solve(b) << endl;

    Matrix2f A, b;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the right hand side b:\n" << b << endl;
    Matrix2f x = A.ldlt().solve(b);
    cout << "The solution is:\n" << x << endl;

    // 计算矩阵的特征值和特征向量
    Matrix2f A;
    A << 1, 2, 2, 3;
    cout << "Here is the matrix A:\n" << A << endl;
    SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
    if (eigensolver.info() != Success) abort();
    cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
    cout << "Here's a matrix whose columns are eigenvectors of A \n"
         << "corresponding to these eigenvalues:\n"
         << eigensolver.eigenvectors() << endl;

    // 计算矩阵的逆和行列式
    Matrix3f A;
    A << 1, 2, 1,
         2, 1, 0,
         -1, 1, 2;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "The determinant of A is " << A.determinant() << endl;
    cout << "The inverse of A is:\n" << A.inverse() << endl;

    // BDCSVD解最小二乘(推荐)
    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
         << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;

    // JacobiSVD解最小二乘
    Eigen::Matrix3f H;
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | 
    Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();

    // AX = 0
    // (AX)`(AX)
    // X`(A`A)X
    // 求特征值和特征向量
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> self_adjoint_solver;
    self_adjoint_solver.compute(ATA);
    Eigen::Matrix3d eigen_values = self_adjoint_solver.eigenvalues().asDiagonal(); // The eigenvalues are sorted in increasing order.
    Eigen::Matrix3d eigen_vectors = self_adjoint_solver.eigenvectors();
    Eigen::Vector3d eigen_vector = eigen_vectors.col(0);
    eigen_values(0, 0) = 0;
    ATA = eigen_vectors * eigen_values * eigen_vectors.inverse();

    Eigen::EigenSolver<Eigen::Matrix4d> general_solver;
    general_solver.compute(ATA);
    cout << "eigenvalues:\n" << general_solver.eigenvalues();
    cout << "eigenvectors:\n" << general_solver.eigenvectors();
    //wxyz = general_solver.eigenvectors().col(0);

    return 0;
}

 参考:Eigen: Linear algebra and decompositions 


CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(test)

set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

find_package(Eigen3)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})

add_executable(eigen_test eigen_test.cc)
target_link_libraries(eigen_test ${Eigen_LIBS})

强制类型转换

Eigen::Matrix4f v1;
const Eigen::Matrix4d v2 = v1.cast<double>();

Eigen::Matrix4d v1;
Eigen::Matrix4f v2 = v1.template cast<float>();

 Eigen::Matrix和cv::Mat相互转换

Eigen::Matrix3d eigen_R;
cv::Mat cv_R;
cv::cv2eigen(cv_R, eigen_R);
cv::eigen2cv(eigen_R, cv_R);

Eigen的SSE兼容,内存分配,和std容器的兼容理解

SSE支持128bit的多指令并行,但是有个要求是处理的对象必须要在内存地址以16byte整数倍的地方开始。不过这些细节Eigen在做并行化的时候会自己处理。但是,如果把一些Eigen的结构放到std的容器里面,比如vector,map。这些容器会把一个一个的Eigen结构在内存里面连续排放。

Eigen提供了两种方法来解决:

1、使用特别的内存分配对象。

std::map<int, Eigen::Vector4f, std::less<int>, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f> > >

std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>>

2、在对象定义的时候,使用特殊的宏,注意必须在所有Eigen对象出现前使用这个宏。

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION

有这个问题的Eigen结构包括:

Eigen::Vector2d
Eigen::Vector4d
Eigen::Vector4f
Eigen::Matrix2d
Eigen::Matrix2f
Eigen::Matrix4d
Eigen::Matrix4f
Eigen::Affine3d
Eigen::Affine3f
Eigen::Quaterniond
Eigen::Quaternionf

另外如果上面提到的这些结构作为一个对象的成员,这个时候需要在类定义里面使用另外一个宏

EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


Eigen库中的Map类

Map类用于通过C++中普通的连续指针或者数组 (raw C/C++ arrays)来构造Eigen里的Matrix类,这就好比Eigen里的Matrix类的数据和raw C++array 共享了一片地址,也就是引用。

1. 比如有个API只接受普通的C++数组,但又要对普通数组进行线性代数操作,那么用它构造为Map类,直接操作Map就等于操作了原始普通数组,省时省力。

2. 再比如有个庞大的Matrix类,在一个大循环中要不断读取Matrix中的一段连续数据,如果你每次都用block operation 去引用数据,太累(虽然block operation 也是引用类型)。于是就事先将这些数据构造成若干Map,那么以后循环中就直接操作Map就行了。

实际上Map类并没有自己申请一片空内存,只是一个引用,所以需要构造时初始化,或者使用Map的指针。

引申一下,Eigen里 ref 类也是引用类型,Armadillo 里 subview 都是引用类型,

Eigen开发人说的

The use ‘sub’ as a Matrix or Map. Actually Map, Ref, and Block inherit from the same base class. You can also use Block.

所以说了这么多,就一句话 Map 就是个引用。


待整理: 

Eigen::Vector3d v1{std::sqrt(2), std::sqrt(2), 0};
Eigen::Vector3d v2{2, 0, 0};
Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(v1, v2); // 返回的旋转矩阵是R*v1 = v2.
Eigen::AngleAxisd v = q;
std::cout << "axis: " << q.axis().transpose() << ", angle: " << q.angle() * 180 / M_PI << std::endl;
Eigen::Matrix3d R = q.toRotationMatrix();
std::cout << "R*v1=\n" << R*v1 << std::endl;

Eigen::Matrix3d cov_inv = cov.inverse();

if (cov_inv.maxCoeff() == std::numeric_limits<float>::infinity() ||

    cov_inv.minCoeff() == -std::numeric_limits<float>::infinity()) {

    return false;

}

求元素的平方

Eigen::Vector3d error;

Eigen::Vector3d abs_error = error.cwiseAbs();

四元素取逆其实就是xyz的值分别取反
q = {x,y,z,w}
q_inverse = {-x,-y,-z ,w}

Eigen::Matrix<float, Eigen::Dynamic, 2> m(10, 2);

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/180462.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • 我做游戏开发这八年[通俗易懂]

    我做游戏开发这八年简述与编程擦肩而过从图像设计到痴迷编程入门编程沉迷编程Java的图形编程就职游戏开发2011,第一次面试&实习生第一个游戏2012年,第二个重量级IP游戏2013年,转战手游与创业2014-2015,第一次创业2015,西山居和多益网络2016-2018,第二次创业2018至今,360游戏艺术(岂凡网络)技术总监简述这篇文章并不是想教会大家如何开发游戏,更不能教大家如…

  • hihoCoder – 1082 – 然而沼跃鱼早就看穿了一切 (字符串处理!!)

    hihoCoder – 1082 – 然而沼跃鱼早就看穿了一切 (字符串处理!!)

  • STM32中IIC协议[通俗易懂]

    IIC协议:接收器和发送器:物理层:SCL–时钟总线SDA–数据总线开始条件:SCL高电平,SDA由高变低停止条件:SCL高电平,SDA由低变高主机:用来产生SCL(发送数据和接收数据)从机:(发送数据和接收数据)总线空闲态:SDA为高电平(没有设备发送开始条件)写:1.主机发送开始条件2.发送从机地址(7位)+写

  • PHP验证邮箱地址代码

    PHP验证邮箱代码:functionisEmail($email){returnstrlen($email)>6&&preg_match("/^[\w\

    2021年12月20日
  • zencart免费模板下载

    zencart免费模板下载最近工作比较忙,没有时间专门来制作这个免费的包包模板。趁国庆放假有时间,顺便就把这个免费模板制作完了。今天特别提供出来给大家下载使用。考虑到很难满足所有有的要求,所以这个模板在一些地方基本没有修改原有模板的布局,只是简单的修改CSS。不过经过修改的这个模板也还算漂亮,大体上的布局已经设置好。我们没有那么多的时间去美化一个这样的模板,俗话说:授人鱼不如授人渔。如果有兴趣做二次开发的朋友可以继续修

  • EL表达式语言_el表达式的语法格式

    EL表达式语言_el表达式的语法格式EL表达式语言1.El简介EL(ExpressionLanguage,表达式语言)是一-种简单的语言,可以方便地访问和处理应用程序数据,而无需使用JSP脚本元素(Scriptlet)或JSP表达式。EL最初是在标准标签库JSTL(JavaServerPageStandardTagLibrary)1.0中定义,从JSTL1.1开始,SUN公司将EL…

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号