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

Fixed Rodrigues/Expmap behavior for small theta

release/4.3a0
Frank Dellaert 2015-07-05 14:03:42 -07:00
parent f9b5bc2936
commit b21d073721
2 changed files with 54 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
gtsam/geometry/SO3.cpp
View File

@ -19,52 +19,69 @@
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <cmath>
using namespace std;
#include <limits>
namespace gtsam {
/* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos;
using std::sin;
// get components of axis \omega
double wx = axis(0), wy = axis(1), wz = axis(2);
double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta;
double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
double C22 = c_1 * wz * wz;
// Near zero, we just have I + skew(omega)
static SO3 FirstOrder(const Vector3& omega) {
Matrix3 R;
R(0, 0) = costheta + C00;
R(1, 0) = wz_sintheta + C01;
R(2, 0) = -wy_sintheta + C02;
R(0, 1) = -wz_sintheta + C01;
R(1, 1) = costheta + C11;
R(2, 1) = wx_sintheta + C12;
R(0, 2) = wy_sintheta + C02;
R(1, 2) = -wx_sintheta + C12;
R(2, 2) = costheta + C22;
R(0, 0) = 1.;
R(1, 0) = omega.z();
R(2, 0) = -omega.y();
R(0, 1) = -omega.z();
R(1, 1) = 1.;
R(2, 1) = omega.x();
R(0, 2) = omega.y();
R(1, 2) = -omega.x();
R(2, 2) = 1.;
return R;
}
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos;
using std::sin;
// get components of axis \omega, where is a unit vector
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
wz_sintheta = wz * sintheta;
const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
const double C22 = c_1 * wz * wz;
Matrix3 R;
R(0, 0) = costheta + C00;
R(1, 0) = wz_sintheta + C01;
R(2, 0) = -wy_sintheta + C02;
R(0, 1) = -wz_sintheta + C01;
R(1, 1) = costheta + C11;
R(2, 1) = wx_sintheta + C12;
R(0, 2) = wy_sintheta + C02;
R(1, 2) = -wx_sintheta + C12;
R(2, 2) = costheta + C22;
return R;
} else {
return FirstOrder(axis*theta);
}
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) {
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) *H = ExpmapDerivative(omega);
if (H)
*H = ExpmapDerivative(omega);
if (omega.isZero())
return Identity();
else {
double angle = omega.norm();
return Rodrigues(omega / angle, angle);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2);
return Rodrigues(omega / theta, theta);
} else {
return FirstOrder(omega);
}
}

2
gtsam/geometry/SO3.h
View File

@ -93,7 +93,7 @@ public:
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);