Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ch7 pose_estimation_3d3d g2o部分代码问题 #277

Open
SIIYIIX opened this issue Mar 7, 2021 · 0 comments
Open

ch7 pose_estimation_3d3d g2o部分代码问题 #277

SIIYIIX opened this issue Mar 7, 2021 · 0 comments

Comments

@SIIYIIX
Copy link

SIIYIIX commented Mar 7, 2021

第二版书中的代码是:
_jacobianOplusXi.block<3,3>(0,0 )= -Eigen::Matrix3d::Identity();
_jacobianOplusXi.block<3,3>(0,3 )= Sophus::SO3::hat(xyz_trans);

运行后的结果:
T=
0.886634 0.141486 -0.440298 -2.14587
-0.460557 0.356687 -0.812811 1.27016
0.0420467 0.923448 0.381413 -0.437775
0 0 0 1

p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-3.478918421023443;
-1.238652237620354;
-0.07315676832163542]

p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.299118, -0.0975683, 1.6558]
(R*p2+t) = [-3.153924541117064;
0.02726650578977896;
0.0910929142018716]

p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-3.378312119547638;
0.4985489194350056;
0.2213095617358987]

p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-3.135972676758195;
0.2908789236581375;
0.2226335813217177]

p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-3.377267677482356;
0.4949932661256665;
0.1588305946736539]

可以发现验证结果完全不对!

正确代码应该是:
_jacobianOplusXi.block<3,3>(0,3 )= -Eigen::Matrix3d::Identity();
_jacobianOplusXi.block<3,3>(0,0 )= Sophus::SO3::hat(xyz_trans);

即把单位矩阵和反对称矩阵换个位置
但是我的理解和书上本来的顺序是一致的即e对ξ的求导顺序应该是书上程序的顺序,希望有大佬能解答一下!

还有个小问题:
bundleAdjustment( pts1, pts2, R, t )函数中的R,t没有实际赋值,验证时采用的是 pose_estimation_3d3d ( pts1, pts2, R, t )求出的R和t

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant