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

matrix block operation

release/4.3a0
Jing Dong 2014-11-09 17:11:11 -05:00
parent 8161cc28ad
commit b0ad350ec4
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Cal3Unified.cpp
View File

@ -78,13 +78,13 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Inlined derivative for calibration // Inlined derivative for calibration
if (H1) { if (H1) {
// part1 // part1
Vector2 DU, DDS2U; Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, // DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2; -ys * sqrt_nx * xi_sqrt_nx2;
DDS2U = H2base * DU;
//*H1 = collect(2, &H1base, &DDS2U); H1->resize(2,10);
*H1 = (Matrix(2,10) << H1base, DDS2U); H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
} }
// Inlined derivative for points // Inlined derivative for points