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

Reformat Unit3

release/4.3a0
Toni 2019-01-15 17:51:38 -05:00
parent 8c8bdc4e67
commit 5c4e5a4b23
1 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/Unit3.cpp
View File

@ -255,24 +255,21 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
const Vector3 xi_hat = basis() * v; const Vector3 xi_hat = basis() * v;
const double theta = xi_hat.norm(); const double theta = xi_hat.norm();
// Treat case of very small v differently // Treat case of very small v differently.
Matrix23 H_from_point; Matrix23 H_from_point;
if (theta < std::numeric_limits<double>::epsilon()) { if (theta < std::numeric_limits<double>::epsilon()) {
const Unit3 exp_p_xi_hat = const Unit3 exp_p_xi_hat = Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat,
Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, H? &H_from_point : nullptr);
H? &H_from_point : nullptr); if (H) { // Jacobian
if (H) { *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + basis());
*H = H_from_point * (-p_ * xi_hat.transpose() * basis() +
basis());
} }
return exp_p_xi_hat; return exp_p_xi_hat;
} }
const Unit3 exp_p_xi_hat = const Unit3 exp_p_xi_hat = Unit3::FromPoint3(
Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), std::cos(theta) * p_ + xi_hat * (sin(theta) / theta),
H? &H_from_point : nullptr); H? &H_from_point : nullptr);
// Jacobian if (H) { // Jacobian
if (H) {
*H = H_from_point * *H = H_from_point *
(p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() +
std::sin(theta) / theta * basis() + std::sin(theta) / theta * basis() +