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

SO(3) is a *Matrix* Lie group. This is it. Rot3 will inherit from this *or* Quaternion.

release/4.3a0
dellaert 2014-12-08 23:58:54 +01:00
parent 118d561e96
commit 8fcf9d9866
4 changed files with 328 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -592,7 +592,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -600,6 +599,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -647,6 +647,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -654,6 +655,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -661,6 +663,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -676,6 +679,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1131,6 +1135,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1360,6 +1365,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1442,7 +1487,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1482,7 +1526,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1490,7 +1533,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1504,46 +1546,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1801,6 +1803,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1808,6 +1811,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1815,6 +1819,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1822,6 +1827,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2194,6 +2200,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSO3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSO3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2692,6 +2706,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2699,6 +2714,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2706,6 +2722,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3257,7 +3274,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

83
gtsam/geometry/SO3.cpp Normal file
View File

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SO3.cpp
* @brief 3*3 matrix representation o SO(3)
* @author Frank Dellaert
* @date December 2014
*/
#include <gtsam/geometry/SO3.h>
#include <cmath>
namespace gtsam {
SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) {
using std::cos;
using std::sin;
// get components of axis \omega
double wx = axis(0), wy = axis(1), wz = axis(2);
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz;
double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz;
double C22 = c_1 * wwTzz;
Matrix3 R;
R << c + C00, -swz + C01, swy + C02, //
swz + C01, c + C11, -swx + C12, //
-swy + C02, swx + C12, c + C22;
return R;
}
Vector3 SO3Chart::Logmap(const SO3& R) {
// note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
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);
// Get trace(R)
double tr = R.trace();
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// 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

101
gtsam/geometry/SO3.h Normal file
View File

@ -0,0 +1,101 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SO3.h
* @brief 3*3 matrix representation o SO(3)
* @author Frank Dellaert
* @date December 2014
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/concepts.h>
#include <cmath>
namespace gtsam {
/**
* True SO(3), i.e., 3*3 matrix subgroup
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
*/
class SO3: public Matrix3 {
protected:
public:
/// Constructor from Eigen Matrix
template<typename Derived>
SO3(const MatrixBase<Derived>& R) :
Matrix3(R.eval()) {
}
/// Constructor from AngleAxisd
SO3(const Eigen::AngleAxisd& angleAxis) :
Matrix3(angleAxis) {
}
};
/// Default Chart for SO3
struct SO3Chart {
typedef SO3 ManifoldType;
/// Exponential map given axis/angle representation of Lie algebra
static SO3 Expmap(const double& theta, const Vector3& w);
/// Exponential map, simply be converting omega to axis/angle representation
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega) {
if (omega.isZero())
return SO3::Identity();
else {
double angle = omega.norm();
return Expmap(angle, omega / angle);
}
}
/// retract, simply be converting omega to AngleAxis
static SO3 Retract(const SO3& p, const Eigen::Ref<const Vector3>& omega) {
return p * Expmap(omega);
}
/// We use our own Logmap, as there is a slight bug in Eigen
static Vector3 Logmap(const SO3& R);
/// local is our own, as there is a slight bug in Eigen
static Vector3 Local(const SO3& q1, const SO3& q2) {
return Logmap(q1.inverse() * q2);
}
};
#define SO3_TEMPLATE
GTSAM_GROUP_IDENTITY0(SO3)
GTSAM_MULTIPLICATIVE_GROUP(SO3_TEMPLATE, SO3)
#define SO3_TANGENT Vector3
#define SO3_CHART SO3Chart
GTSAM_MANIFOLD(SO3_TEMPLATE, SO3, 3, SO3_TANGENT, SO3_CHART)
/// Define SO3 to be a model of the Lie Group concept
namespace traits {
template<>
struct structure_category<SO3> {
typedef lie_group_tag type;
};
}
} // end namespace gtsam

83
gtsam/geometry/tests/testSO3.cpp Normal file
View File

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testQuaternion.cpp
* @brief Unit tests for SO3, as a GTSAM-adapted Lie Group
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
// BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
}
//******************************************************************************
TEST(SO3 , Constructor) {
SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
//******************************************************************************
TEST(SO3 , Invariants) {
SO3 q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
SO3 q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
// group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!)
}
//******************************************************************************
TEST(SO3 , Local) {
Vector3 z_axis(0, 0, 1);
SO3 q1(Eigen::AngleAxisd(0, z_axis));
SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<SO3>::type Chart;
Vector3 expected(0, 0, 0.1);
Vector3 actual = Chart::Local(q1, q2);
EXPECT(assert_equal((Vector)expected,actual));
}
//******************************************************************************
TEST(SO3 , Retract) {
Vector3 z_axis(0, 0, 1);
SO3 q(Eigen::AngleAxisd(0, z_axis));
SO3 expected(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<SO3>::type Chart;
Vector3 v(0, 0, 0.1);
SO3 actual = Chart::Retract(q, v);
EXPECT(actual.isApprox(expected));
}
//******************************************************************************
TEST(SO3 , Compose) {
}
//******************************************************************************
TEST(SO3 , Between) {
}
//******************************************************************************
TEST(SO3 , Inverse) {
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************