CamLaserCalibraTool icon indicating copy to clipboard operation
CamLaserCalibraTool copied to clipboard

PoseLocalParameterization中四元数更新代码错误?

Open Deephome opened this issue 3 years ago • 4 comments

在pose_local_parameterization.cpp的deltaQ函数中:

Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta) { Eigen::Quaterniond dq; Eigen::Vector3d half_theta = theta; // theta: so(3) 旋转向量 half_theta /= static_cast<double>(2.0); // half_theta: 四元数向量部分 dq.w() = static_cast<double>(1.0); dq.x() = half_theta.x(); dq.y() = half_theta.y(); dq.z() = half_theta.z(); return dq; // q增量 } 看了一些博客,了解到其中theta/2对应纯虚四元数向量,应该经过指数变换得到对应的四元数dq。但这里,直接将实部w赋值为1,未进行指数变换。

哪位大神可以指导一下?

参考:https://blog.csdn.net/hzwwpgmwy/article/details/86490556

Deephome avatar May 13 '21 08:05 Deephome

@Deephome 四元数里没有指数变换的概念,指数变换是李群李代数的概念。 一个四元数 q = [cos(θ/2), sin(θ/2)*nx, sin(θ/2)*ny, sin(θ/2)*nz],其中旋转轴为n = (nx, ny, nz),旋转角为θ. 当旋转角θ为小量,则cos(θ/2)≈1,sin(θ/2)≈θ/2。即q≈[1,(θ/2)*nx, (θ/2)*ny,(θ/2)*nz ]。

TouchDeeper avatar May 13 '21 08:05 TouchDeeper

@TouchDeeper 谢谢你的回复!当theta很小,近似确实是这样的,如果为了计算简单,那就说的通了。

但是,请你看下下面这个博客的1.3节,确实存在纯虚四元数的指数映射 参考:四元数

Deephome avatar May 13 '21 10:05 Deephome

@Deephome 草率了,看来四元数里也有指数变换的概念,谢谢哈

TouchDeeper avatar May 13 '21 11:05 TouchDeeper

@Deephome 草率了,看来四元数里也有指数变换的概念,谢谢哈

你好,能有个联系方式么,这个代码我有些地方不明白,能交流一下不,代码LaseCamCalCeres.cpp中57行,在计算雅克比矩阵的时候 jaco_i.rightCols<3>() = planar_.head(3).transpose() * (-qcl.toRotationMatrix() * skewSymmetric(point_)); 这边我有点不理解? 我的理解雅克比矩阵应该这样计算:jaco_i.rightCols<3>() = planar_.head(3).transpose() *-skewSymmetric( qcl.toRotationMatrix() * point_+tcl);

woshicver avatar May 03 '22 03:05 woshicver