Eigen计算坐标系变换

最新推荐文章于 2024-07-16 12:00:00 发布

小弧光

最新推荐文章于 2024-07-16 12:00:00 发布

阅读量4.1k

点赞数

分类专栏：算法文章标签： Eigen

本文链接：https://blog.csdn.net/itfanr/article/details/83060544

版权

#include <vector>
#include <Eigen/Geometry>
#include <stdio.h>
#include <iostream>
#define Scalar double

 Eigen::Transform<Scalar, 3, Eigen::Affine> findTransformBetween2CS(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
                                        const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
                                        const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
                                        const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y)
{

// Convert lines into Vector3 :
  Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
                              fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
                              fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),

                              to0 (to_line_x.template h