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

remove unnecessary malloc

release/4.3a0
thduynguyen 2014-12-25 08:53:53 -05:00
parent 5d0e8f202f
commit 34592f21c4
1 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/Pose2.cpp
View File

@ -150,17 +150,16 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
*/ */
double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
J = (Matrix(3,3) << J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, 0, 0, 1;
0, 0, 1).finished();
} }
else { else {
// Thanks to Krunal: Apply L'Hospital rule to several times to // Thanks to Krunal: Apply L'Hospital rule to several times to
// compute the limits when alpha -> 0 // compute the limits when alpha -> 0
J = (Matrix(3,3) << 1,0,-0.5*v[1], J << 1,0,-0.5*v[1],
0,1, 0.5*v[0], 0,1, 0.5*v[0],
0,0, 1).finished(); 0,0, 1;
} }
return J; return J;
@ -175,15 +174,14 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
double v1 = v[0], v2 = v[1]; double v1 = v[0], v2 = v[1];
J = (Matrix(3,3) << J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, 0, 0, 1;
0, 0, 1).finished();
} }
else { else {
J = (Matrix(3,3) << 1,0, 0.5*v[1], J << 1,0, 0.5*v[1],
0,1, -0.5*v[0], 0,1, -0.5*v[0],
0,0, 1).finished(); 0,0, 1;
} }
return J; return J;
} }