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

Fixed bug in BetweenFactor

release/4.3a0
dellaert 2014-12-22 11:25:21 +01:00
parent 7f2876f456
commit 9a22f3a825
2 changed files with 19 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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

16
gtsam/slam/BetweenFactor.h
View File

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