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 // Compute the 3D xi_hat vector
const Vector3 xi_hat = basis() * v; const Vector3 xi_hat = basis() * v;
const double theta = xi_hat.norm(); const double theta = xi_hat.norm();
const double c = std::cos(theta);
// 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 = 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); H? &H_from_point : nullptr);
if (H) { // Jacobian 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; return exp_p_xi_hat;
} }
const Unit3 exp_p_xi_hat = Unit3::FromPoint3( const double st = std::sin(theta) / theta;
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
H? &H_from_point : nullptr); H? &H_from_point : nullptr);
if (H) { // Jacobian if (H) { // Jacobian
*H = H_from_point * *H = H_from_point *
(p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
std::sin(theta) / theta * basis() + xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2))
* xi_hat.transpose() / theta * basis());
} }
return exp_p_xi_hat; return exp_p_xi_hat;
} }