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>
<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>

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 */