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

Optimize retract function for Unit3

release/4.3a0
Toni 2019-01-21 16:13:07 -05:00
parent 8039ed94af
commit e63ceee938
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/Unit3.cpp
View File

@ -254,27 +254,27 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
// Compute the 3D xi_hat vector
const Vector3 xi_hat = basis() * v;
const double theta = xi_hat.norm();
const double c = std::cos(theta);
// Treat case of very small v differently.
Matrix23 H_from_point;
if (theta < std::numeric_limits<double>::epsilon()) {
const Unit3 exp_p_xi_hat = Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat,
const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
H? &H_from_point : nullptr);
if (H) { // Jacobian
*H = H_from_point * (-p_ * xi_hat.transpose() * basis() + basis());
*H = H_from_point *
(-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
}
return exp_p_xi_hat;
}
const Unit3 exp_p_xi_hat = Unit3::FromPoint3(
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta),
H? &H_from_point : nullptr);
const double st = std::sin(theta) / theta;
const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
H? &H_from_point : nullptr);
if (H) { // Jacobian
*H = H_from_point *
(p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() +
std::sin(theta) / theta * basis() +
xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2))
* xi_hat.transpose() / theta * basis());
(p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
}
return exp_p_xi_hat;
}