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

Merge branch 'feature/tighteningTraits' of https://bitbucket.org/gtborg/gtsam into feature/tighteningTraits

release/4.3a0
Mike Bosse 2014-12-22 11:33:58 +01:00
parent 4a708c855f 9a22f3a825
commit 77fbe5cabc
3 changed files with 65 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2999,6 +2999,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

46
gtsam/geometry/tests/testRot3.cpp
View File

@ -38,6 +38,13 @@ static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
//******************************************************************************
TEST(Rot3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Rot3 >));
BOOST_CONCEPT_ASSERT((IsManifold<Rot3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3 >));
}
/* ************************************************************************* */
TEST( Rot3, chart)
{
@ -601,6 +608,45 @@ TEST( Rot3, slerp)
EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
}
//******************************************************************************
TEST(Rot3 , Traits) {
Rot3 R1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
check_group_invariants(R1, R2);
check_manifold_invariants(R1, R2);
Rot3 expected, actual;
Matrix actualH1, actualH2;
Matrix numericalH1, numericalH2;
expected = R1 * R2;
actual = traits_x<Rot3>::Compose(R1, R2, actualH1, actualH2);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative21(traits_x<Rot3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
numericalH2 = numericalDerivative22(traits_x<Rot3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
expected = R1.inverse() * R2;
actual = traits_x<Rot3>::Between(R1, R2, actualH1, actualH2);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative21(traits_x<Rot3>::Between, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
numericalH2 = numericalDerivative22(traits_x<Rot3>::Between, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
expected = R1.inverse();
actual = traits_x<Rot3>::Inverse(R1, actualH1);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative11(traits_x<Rot3>::Inverse, R1);
EXPECT(assert_equal(numericalH1,actualH1));
}
/* ************************************************************************* */
int main() {
TestResult tr;

16
gtsam/slam/BetweenFactor.h
View File

@ -93,12 +93,18 @@ namespace gtsam {
boost::none) const {
T hx = traits_x<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
typename traits_x<T>::ChartJacobian Hlocal;
//OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
Vector rval = traits_x<T>::Local(measured_, hx, boost::none, Hlocal);
(*H1) = ((*Hlocal) * (*H1)).eval();
(*H2) = ((*Hlocal) * (*H2)).eval();
#ifdef BETWEENFACTOR_ASSUME_SMALL
return traits_x<T>::Local(measured_, hx);
#else
return traits_x<T>::Local(measured_, hx);
static const int N = traits_x<T>::dimension;
Eigen::Matrix<double,N,N> Hlocal;
Vector rval = traits_x<T>::Local(measured_, hx, boost::none,
(H1 || H2) ? &Hlocal : 0);
if (H1) *H1 = Hlocal * (*H1);
if (H1) *H2 = Hlocal * (*H2);
return rval;
#endif
}
/** return the measured */