12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1206 from huanglilong/fix_typo

release/4.3a0
Varun Agrawal 2022-05-26 10:54:04 -04:00 committed by GitHub
parent da00870ee1 f95b449904
commit 725f485a09
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Rot3.cpp
View File

@ -292,8 +292,8 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
(*H)(1, 8) = yHb22 * cx;
// Next, calculate the derivate of z. We have
// c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy
// c22 = a11 * cx - a12 * sx
// c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
// c11 = a11 * cx - a12 * sx
const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);