slam中地图地图原点坐标系或world或grid坐标系在map坐标系下的坐标计算
Matrix<double, 4, 4> m_world_to_map; //amcl3 地图world或grid坐标系或地图原点坐标系在map坐标系下的关系,即: mapTgrid = mapTworld
ros::Subscriber m_sub_map_topic = m_node_handle.subscribe("/map", 1, &NeoLocalizationNode::map_callback, this);
void map_callback(const nav_msgs::OccupancyGrid::ConstPtr& ros_map)
{
std::lock_guard<std::mutex> lock(m_node_mutex);
ROS_INFO_STREAM("NeoLocalizationNode: Got new map with dimensions " << ros_map->info.width << " x " << ros_map->info.height
<< " and cell size " << ros_map->info.resolution);
{
tf::Transform tmp;
//地图原点，小车启点
tf::poseMsgToTF(ros_map->info.origin, tmp);
//mapTgrid = mapTworld
m_world_to_map = convert_transform_25(tmp);
}
}计算小车base_link在world或grid坐标系或地图原点坐标系下的坐标
const Matrix<double, 4,4>gridTbase_link=mapTgrid.inverse()*convert_transform_25(mapTodom)*odomTbase_link;/*
amcl3
* Converts ROS 3D Transform to a 2.5D matrix.
* 这个函数的目的是将 ROS 的 3D 变换表示（由 tf::Transform 类型表示）转换为一个 4x4 的矩阵。在这个转换中，
* Z 轴方向的旋转角度被映射到了矩阵的第三行第三列，而其他变换分量则映射到了矩阵的其他位置。
*/
inline
Matrix<double, 4, 4> convert_transform_25(const tf::Transform& trans)
{
Matrix<double, 4, 4> res;// 创建一个 4x4 的双精度浮点型矩阵
将变换的旋转部分的 X、Y 分量分别存入矩阵的第一行和第二行
res(0, 0) = trans.getBasis()[0][0];
res(1, 0) = trans.getBasis()[1][0];
// // 将变换的旋转部分的 X、Y 分量分别存入矩阵的第二行和第一行（交换）
res(0, 1) = trans.getBasis()[0][1];
res(1, 1) = trans.getBasis()[1][1];
// 将变换的平移部分的 X、Y、Z 分量分别存入矩阵的第一、二、三行的第四列
res(0, 3) = trans.getOrigin()[0];
res(1, 3) = trans.getOrigin()[1];
res(2, 3) = tf::getYaw(trans.getRotation());// 获取变换的旋转部分的偏航角并存入矩阵的第三行第四列
res(2, 2) = 1;// 设置矩阵的第三行第三列为1（表示常量）
res(3, 3) = 1;// 设置矩阵的第四行第四列为1（表示常量）
return res;// 返回转换后的矩阵
}
/*
* Converts ROS 3D Transform to a 3D matrix.
* * 这个函数的目的是将 ROS 的 3D 变换表示（由 tf::Transform 类型表示）转换为一个 4x4 的矩阵。在这个转换中，
* 变换的旋转部分的矩阵元素被映射到矩阵的前三行前三列，而变换的平移部分则映射到矩阵的第一、二、三行的第四列。
*/
inline
Matrix<double, 4, 4> convert_transform_3(const tf::Transform& trans)
{
Matrix<double, 4, 4> res; 创建一个 4x4 的双精度浮点型矩阵
使用双重循环将变换的旋转部分的矩阵元素存入矩阵的前三行前三列
for(int j = 0; j < 3; ++j) {
for(int i = 0; i < 3; ++i) {
res(i, j) = trans.getBasis()[i][j];
}
}
// 将变换的平移部分的 X、Y、Z 分量分别存入矩阵的第一、二、三行的第四列
res(0, 3) = trans.getOrigin()[0];
res(1, 3) = trans.getOrigin()[1];
res(2, 3) = trans.getOrigin()[2];
res(3, 3) = 1;// 设置矩阵的第四行第四列为1（表示常量）
return res; // 返回转换后的矩阵
}矩阵运算
#ifndef INCLUDE_MATH_MATRIX_H_
#define INCLUDE_MATH_MATRIX_H_
#include <cmath>
#include <cstdint>
#include <ostream>
#include <array>
#include <stdexcept>
#include <initializer_list>
template<typename T>
class MatrixX;
struct NO_INIT {};
template<typename T, size_t Rows, size_t Cols>
class Matrix;
template<typename T, size_t Rows, size_t Cols>
Matrix<T, Rows, Cols> inverse(const Matrix<T, Rows, Cols>& mat);
template<typename T>
Matrix<T, 1, 1> inverse(const Matrix<T, 1, 1>& m);
template<typename T>
Matrix<T, 2, 2> inverse(const Matrix<T, 2, 2>& m);
template<typename T>
Matrix<T, 3, 3> inverse(const Matrix<T, 3, 3>& m);
template<typename T>
Matrix<T, 4, 4> inverse(const Matrix<T, 4, 4>& m);
template<typename T, size_t Rows, size_t Cols>
class Matrix {
public:
typedef T Scalar;
/*
* Default constructor initializes with all zeros.
*/
Matrix() : data({}) {}
/*
* Special constructor, does not initialize data.
*/
Matrix(NO_INIT) {}
/*
* Generic copy constructor.
*/
template<typename S>
Matrix(const Matrix<S, Rows, Cols>& mat) {
for(size_t i = 0; i < size(); ++i) {
data[i] = mat.data[i];
}
}
/*
* Generic copy constructor for a MatrixX.
*/
template<typename S>
Matrix(const MatrixX<S>& mat);
/*
* Initialize with column major list (ie. human readable form).
*/
Matrix(const std::initializer_list<T>& list) {
if(list.size() != Rows * Cols) {
throw std::logic_error("list.size() != Rows * Cols");
}
size_t i = 0;
for(const T& v : list) {
(*this)(i / Cols, i % Cols) = v;
i++;
}
}
/*
* Returns identity matrix.
*/
static Matrix Identity() {
if(Rows != Cols) {
throw std::logic_error("Rows != Cols");
}
Matrix res;
for(size_t i = 0; i < Rows; ++i) {
res(i, i) = 1;
}
return res;
}
T* get_data() {
return &data[0];
}
const T* get_data() const {
return &data[0];
}
T& operator()(size_t i, size_t j) {
return data[j * Rows + i];
}
const T& operator()(size_t i, size_t j) const {
return data[j * Rows + i];
}
T& operator[](size_t i) {
return data[i];
}
const T& operator[](size_t i) const {
return data[i];
}
Matrix<T, Cols, Rows> transpose() const {
Matrix<T, Cols, Rows> res;
for(size_t j = 0; j < Cols; ++j) {
for(size_t i = 0; i < Rows; ++i) {
res(j, i) = (*this)(i, j);
}
}
return res;
}
T squared_norm() const {
T sum = 0;
for(T v : data) {
sum += v*v;
}
return sum;
}
T norm() const {
return std::sqrt(squared_norm());
}
size_t rows() const {
return Rows;
}
size_t cols() const {
return Cols;
}
size_t size() const {
return Rows * Cols;
}
Matrix inverse() const {
if(Rows != Cols) {
throw std::logic_error("Rows != Cols");
}
return ::inverse<T>(*this);
}
template<size_t N, size_t M = 1>
Matrix<T, N, M> get(size_t i_0 = 0, size_t j_0 = 0) const {
Matrix<T, N, M> res;
for(size_t j = 0; j < M; ++j) {
for(size_t i = 0; i < N; ++i) {
res(i, j) = (*this)(i + i_0, j + j_0);
}
}
return res;
}
Matrix<T, Rows+1, 1> extend() const {
if(Cols != 1) {
throw std::logic_error("Cols != 1");
}
Matrix<T, Rows+1, 1> res;
for(size_t i = 0; i < Rows; ++i) {
res[i] = data[i];
}
res[Rows] = 1;
return res;
}
Matrix<T, Rows-1, 1> project() const {
if(Cols != 1) {
throw std::logic_error("Cols != 1");
}
auto res = get<Rows-1>();
if(data[Rows-1] != 1) {
res *= T(1) / data[Rows-1];
}
return res;
}
void normalize() {
(*this) *= (T(1) / norm());
}
Matrix normalized() const {
return (*this) * (T(1) / norm());
}
T dot(const Matrix& B) const {
T res = 0;
for(size_t i = 0; i < size(); ++i) {
res += data[i] * B[i];
}
return res;
}
template<size_t N>
Matrix<T, Rows, N> operator*(const Matrix<T, Cols, N>& B) const {
Matrix<T, Rows, N> C;
for(size_t i = 0; i < Rows; ++i) {
for(size_t j = 0; j < N; ++j) {
for(size_t k = 0; k < Cols; ++k) {
C(i, j) += (*this)(i, k) * B(k, j);
}
}
}
return C;
}
template<size_t N>
Matrix& operator*=(const Matrix<T, Cols, N>& B) {
*this = *this * B;
return *this;
}
Matrix& operator*=(const T& value) {
for(size_t i = 0; i < Rows * Cols; ++i) {
(*this)[i] *= value;
}
return *this;
}
Matrix operator*(const T& value) const {
Matrix C = *this;
C *= value;
return C;
}
Matrix& operator/=(const T& value) {
for(size_t i = 0; i < Rows * Cols; ++i) {
(*this)[i] /= value;
}
return *this;
}
Matrix operator/(const T& value) const {
Matrix C = *this;
C /= value;
return C;
}
Matrix& operator+=(const Matrix& B) {
for(size_t i = 0; i < Rows * Cols; ++i) {
(*this)[i] += B[i];
}
return *this;
}
Matrix operator+(const Matrix& B) const {
Matrix C = *this;
C += B;
return C;
}
Matrix& operator-=(const Matrix& B) {
for(size_t i = 0; i < Rows * Cols; ++i) {
(*this)[i] -= B[i];
}
return *this;
}
Matrix operator-(const Matrix& B) const {
Matrix C = *this;
C -= B;
return C;
}
bool operator==(const Matrix& B) const {
return data == B.data;
}
bool operator!=(const Matrix& B) const {
return data != B.data;
}
std::ostream& print(std::ostream& out, const std::string& name) const {
out << name << " = [" << std::endl;
for(size_t i = 0; i < Rows; ++i) {
if(i > 0) {
out << "," << std::endl;
}
out << "[";
for(size_t j = 0; j < Cols; ++j) {
if(j > 0) {
out << ", ";
}
out << (*this)(i, j);
}
out << "]";
}
out << "]" << std::endl;
return out;
}
std::array<T, Rows * Cols> data;
};
template<typename T, size_t Rows, size_t Cols>
Matrix<T, Rows, Cols> inverse(const Matrix<T, Rows, Cols>& mat) {
throw std::logic_error("not implemented");
}
template<typename T>
Matrix<T, 1, 1> inverse(const Matrix<T, 1, 1>& m) {
return m * (T(1) / m[0]);
}
template<typename T>
Matrix<T, 2, 2> inverse(const Matrix<T, 2, 2>& m) {
const T det = (m(0,0) * m(1, 1)) - (m(0, 1) * m(1, 0));
if(det == 0) {
throw std::runtime_error("inverse(): determinant = 0");
}
Matrix<T, 2, 2> tmp;
tmp(0, 0) = m(1, 1);
tmp(1, 0) = -m(1, 0);
tmp(0, 1) = -m(0, 1);
tmp(1, 1) = m(0, 0);
return tmp * (T(1) / det);
}
template<typename T>
Matrix<T, 3, 3> inverse(const Matrix<T, 3, 3>& m) {
const T det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
if(det == 0) {
throw std::runtime_error("inverse(): determinant = 0");
}
Matrix<T, 3, 3> tmp;
tmp(0, 0) = m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2);
tmp(0, 1) = m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2);
tmp(0, 2) = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
tmp(1, 0) = m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2);
tmp(1, 1) = m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0);
tmp(1, 2) = m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2);
tmp(2, 0) = m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1);
tmp(2, 1) = m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1);
tmp(2, 2) = m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1);
return tmp * (T(1) / det);
}
template<typename T>
Matrix<T, 4, 4> inverse(const Matrix<T, 4, 4>& m) {
Matrix<T, 4, 4> tmp;
tmp[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15]
+ m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
tmp[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15]
- m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
tmp[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15]
+ m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
tmp[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14]
- m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
tmp[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15]
- m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
tmp[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15]
+ m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
tmp[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15]
- m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
tmp[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14]
+ m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
tmp[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15]
+ m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
tmp[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15]
- m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
tmp[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15]
+ m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
tmp[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14]
- m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
tmp[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11]
- m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
tmp[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11]
+ m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
tmp[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11]
- m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
tmp[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10]
+ m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
const T det = m[0] * tmp[0] + m[1] * tmp[4] + m[2] * tmp[8] + m[3] * tmp[12];
if(det == 0) {
throw std::runtime_error("inverse(): determinant = 0");
}
return tmp * (T(1) / det);
}
#endif /* INCLUDE_MATH_MATRIX_H_ */
3670
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
