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

SO3 now Lie group again!

release/4.3a0
dellaert 2014-12-27 13:37:15 +01:00
parent eb5d619729
commit a25b2f14d6
4 changed files with 196 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
.cproject
View File

@ -592,6 +592,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -599,6 +600,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -646,6 +648,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -653,6 +656,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -660,6 +664,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -675,6 +680,7 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1130,6 +1136,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1359,6 +1366,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1441,7 +1488,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1481,7 +1527,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1489,7 +1534,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1503,46 +1547,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1800,6 +1804,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1807,6 +1812,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1814,6 +1820,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1821,6 +1828,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2185,10 +2193,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testQuaternion.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSO3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j4</buildArguments>
<buildTarget>testQuaternion.run</buildTarget> <buildTarget>testSO3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2305,14 +2313,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSimpleHelicopter.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSimpleHelicopter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments> <buildArguments>-j1</buildArguments>
@ -2699,6 +2699,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2706,6 +2707,7 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2713,6 +2715,7 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2806,6 +2809,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testAdaptAutoDiff.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCallRecord.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testCallRecord.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -3190,14 +3201,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testAdaptAutoDiff.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j4</buildArguments>
@ -3256,7 +3259,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>

105
gtsam/geometry/SO3.cpp
View File

@ -17,6 +17,7 @@
*/ */
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <cmath> #include <cmath>
namespace gtsam { namespace gtsam {
@ -44,54 +45,60 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
return R; return R;
} }
// /// simply convert omega to axis/angle representation
//namespace lie_group { SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
///// simply convert omega to axis/angle representation ChartJacobian H) {
//template <>
//SO3 expmap<SO3>(const Eigen::Ref<const Vector3>& omega) { if (H)
// if (omega.isZero()) CONCEPT_NOT_IMPLEMENTED;
// return SO3::Identity();
// else { if (omega.isZero())
// double angle = omega.norm(); return SO3::Identity();
// return Rodrigues(angle, omega / angle); else {
// } double angle = omega.norm();
//} return Rodrigues(angle, omega / angle);
// }
//template <> }
//Vector3 logmap<SO3>(const SO3& R) {
// Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// // note switch to base 1 using std::sqrt;
// const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); using std::sin;
// const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
// const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); if (H)
// CONCEPT_NOT_IMPLEMENTED;
// // Get trace(R)
// double tr = R.trace(); // note switch to base 1
// const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
// // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
// // we do something special const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// if (std::abs(tr + 1.0) < 1e-10) {
// if (std::abs(R33 + 1.0) > 1e-10) // Get trace(R)
// return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); double tr = R.trace();
// else if (std::abs(R22 + 1.0) > 1e-10)
// return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// else // we do something special
// // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit if (std::abs(tr + 1.0) < 1e-10) {
// return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); if (std::abs(R33 + 1.0) > 1e-10)
// } else { return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
// double magnitude; else if (std::abs(R22 + 1.0) > 1e-10)
// double tr_3 = tr - 3.0; // always negative return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
// if (tr_3 < -1e-7) { else
// double theta = acos((tr - 1.0) / 2.0); // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
// magnitude = theta / (2.0 * sin(theta)); return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
// } else { } else {
// // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) double magnitude;
// // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) double tr_3 = tr - 3.0; // always negative
// magnitude = 0.5 - tr_3 * tr_3 / 12.0; if (tr_3 < -1e-7) {
// } double theta = acos((tr - 1.0) / 2.0);
// return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); magnitude = theta / (2.0 * sin(theta));
// } } else {
//} // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
//} // end namespace lie_group // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
}
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
}
} // end namespace gtsam } // end namespace gtsam

30
gtsam/geometry/SO3.h
View File

@ -30,13 +30,17 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold. * We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it... * However, round-off errors in repeated composition could move off it...
*/ */
class SO3: public Matrix3 { class SO3: public Matrix3, public LieGroup<SO3,3> {
protected: protected:
public: public:
enum { dimension=3 }; enum { dimension=3 };
/// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) {
}
/// Constructor from Eigen Matrix /// Constructor from Eigen Matrix
template<typename Derived> template<typename Derived>
SO3(const MatrixBase<Derived>& R) : SO3(const MatrixBase<Derived>& R) :
@ -48,10 +52,6 @@ public:
Matrix3(angleAxis) { Matrix3(angleAxis) {
} }
static SO3 identity() {
return I_3x3;
}
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << s << *this << std::endl; std::cout << s << *this << std::endl;
} }
@ -60,6 +60,26 @@ public:
return equal_with_abs_tol(*this, R, tol); return equal_with_abs_tol(*this, R, tol);
} }
static SO3 identity() { return I_3x3; }
SO3 inverse() const { return this->Matrix3::inverse(); }
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none);
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
Matrix3 AdjointMap() const { return *this; }
// Chart at origin
struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
return Expmap(v,H);
}
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
return Logmap(R,H);
}
};
using LieGroup<SO3,3>::inverse;
}; };
template<> template<>

93
gtsam/geometry/tests/testSO3.cpp
View File

@ -16,19 +16,18 @@
**/ **/
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef OptionalJacobian<3,3> SO3Jacobian;
//****************************************************************************** //******************************************************************************
TEST(SO3 , Concept) { TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
// BOOST_CONCEPT_ASSERT((IsManifold<SO3 >)); BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
// BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
} }
//****************************************************************************** //******************************************************************************
@ -37,84 +36,94 @@ TEST(SO3 , Constructor) {
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Invariants) { SO3 id;
SO3 q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); Vector3 z_axis(0, 0, 1);
SO3 q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
// group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
}
#if 0
//****************************************************************************** //******************************************************************************
TEST(SO3 , Local) { TEST(SO3 , Local) {
Vector3 z_axis(0, 0, 1);
SO3 q1(Eigen::AngleAxisd(0, z_axis));
SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
SO3Jacobian H1,H2;
Vector3 expected(0, 0, 0.1); Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<SO3>::Local(q1, q2, H1, H2); Vector3 actual = traits<SO3>::Local(R1, R2);
EXPECT(assert_equal((Vector)expected,actual)); EXPECT(assert_equal((Vector)expected,actual));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Retract) { TEST(SO3 , Retract) {
Vector3 z_axis(0, 0, 1);
SO3 q(Eigen::AngleAxisd(0, z_axis));
SO3 expected(Eigen::AngleAxisd(0.1, z_axis));
Vector3 v(0, 0, 0.1); Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(q, v); SO3 actual = traits<SO3>::Retract(R1, v);
EXPECT(actual.isApprox(expected)); EXPECT(actual.isApprox(R2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Compose) { TEST(SO3 , Compose) {
Vector3 z_axis(0, 0, 1); SO3 expected = R1 * R2;
SO3 q1(Eigen::AngleAxisd(0.2, z_axis));
SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
SO3 expected = q1 * q2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Compose(q1, q2, actualH1, actualH2); SO3 actual = traits<SO3>::Compose(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual)); EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Compose, q1, q2); Matrix numericalH1 = numericalDerivative21(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Compose, q1, q2); Matrix numericalH2 = numericalDerivative22(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Between) { TEST(SO3 , Between) {
Vector3 z_axis(0, 0, 1); SO3 expected = R1.inverse() * R2;
SO3 q1(Eigen::AngleAxisd(0.2, z_axis));
SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
SO3 expected = q1.inverse() * q2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Between(q1, q2, actualH1, actualH2); SO3 actual = traits<SO3>::Between(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual)); EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Between, q1, q2); Matrix numericalH1 = numericalDerivative21(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Between, q1, q2); Matrix numericalH2 = numericalDerivative22(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Inverse) { TEST(SO3 , Inverse) {
Vector3 z_axis(0, 0, 1);
SO3 q1(Eigen::AngleAxisd(0.1, z_axis));
SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); SO3 expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH; Matrix actualH;
SO3 actual = traits<SO3>::Inverse(q1, actualH); SO3 actual = traits<SO3>::Inverse(R1, actualH);
EXPECT(traits<SO3>::Equals(expected,actual)); EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH = numericalDerivative11(traits<SO3>::Inverse, q1); Matrix numericalH = numericalDerivative11(traits<SO3>::Inverse, R1);
EXPECT(assert_equal(numericalH,actualH)); EXPECT(assert_equal(numericalH,actualH));
} }
#endif
//******************************************************************************
TEST(SO3 , Invariants) {
check_group_invariants(id,id);
check_group_invariants(id,R1);
check_group_invariants(R2,id);
check_group_invariants(R2,R1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
//TEST(SO3 , LieGroupDerivatives) {
// CHECK_LIE_GROUP_DERIVATIVES(id,id);
// CHECK_LIE_GROUP_DERIVATIVES(id,R2);
// CHECK_LIE_GROUP_DERIVATIVES(R2,id);
// CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
//}
//******************************************************************************
TEST(SO3 , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,R2);
CHECK_CHART_DERIVATIVES(R2,id);
CHECK_CHART_DERIVATIVES(R2,R1);
}
//****************************************************************************** //******************************************************************************
int main() { int main() {