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)
blank

相关推荐

  • 查看Linux版本

    查看Linux版本

    2021年10月18日
  • ES6中的模板字符串改变html_vue事件绑定修饰符

    ES6中的模板字符串改变html_vue事件绑定修饰符step1:模板字符传中写法renderDom(){return`<divdata-action=”goDetail”data-url=”${item.clickUrl}”class=”uliao-news-item”><h1>${item.title}</h1>&…

  • 简单易学的机器学习算法——Mean Shift聚类算法

    简单易学的机器学习算法——Mean Shift聚类算法参考文献MeanShiftClusteringMeanshift,聚类算法

  • PLSQL 安装教程「建议收藏」

    PLSQL 安装教程「建议收藏」PLSQL安装教程1、首先要有oracle数据库或者有oracle服务器,才可以实现使用PLSQLDeveloper工具连接到oracle数据库进行开发.2、新建文件夹,将客户端文件放到里面D:\khd\instantclient_11_2,在文件夹instantclient_11_2中自己新建NETWORK文件夹,进入NETWORK文件夹新建ADMIN文件夹,将tnsn…

  • 学会使用getopt函数[通俗易懂]

    学会使用getopt函数[通俗易懂]简介getopt函数是命令行参数解析函数,在平时阅读源码的时候经常遇到,很有必要对其总结一下,做个记录!命令行参数各组成部分的名称先来了解下命令行参数各组成部分的名称。直接上图:非常清楚,命令行参数由Commandname,Option,Optionargument以及Operands组成。Commandname不用多说,就是程序的名称。操作对象Operands又…

  • nginx配置访问本地静态资源

    nginx配置访问本地静态资源nginx作为一款高性能的服务器,用途很多,除了可以做后端服务器的代理,负载均衡之外你,还有一个用途就是做静态资源的缓存服务器,比如在前后端分离的项目中,为了加速前端页面的响应速度,我们可以将前端的相关资源,例如html,js,css或者图片等放到nginx指定的目录下,访问的时候只需要通过IP加路径就可以实现高效快速的访问,下面说说如何在windows下使用nginx作为静态资源服务器,1、……

发表回复

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

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