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

Fixed bug in equality check

release/4.3a0
Alex Cunningham 2012-08-27 01:40:11 +00:00
parent 12290dc7b2
commit 6b14e652ff
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/nonlinear/LinearContainerFactor.cpp
View File

@ -117,11 +117,13 @@ void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyF
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const { bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f); const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f);
if (!jcf || factor_->equals(*jcf->factor_, tol) || NonlinearFactor::equals(f)) if (!jcf || !factor_->equals(*jcf->factor_, tol) || !NonlinearFactor::equals(f))
return false; return false;
if (!linearizationPoint_ && !jcf->linearizationPoint_) if (!linearizationPoint_ && !jcf->linearizationPoint_)
return true; return true;
return jcf->linearizationPoint_ && linearizationPoint_->equals(*jcf->linearizationPoint_, tol); if (linearizationPoint_ && jcf->linearizationPoint_)
return linearizationPoint_->equals(*jcf->linearizationPoint_, tol);
return false;
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -9,6 +9,7 @@
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h> #include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
using namespace gtsam; using namespace gtsam;
@ -95,6 +96,11 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
values.insert(x2, poseA2); values.insert(x2, poseA2);
LinearContainerFactor actFactor(expLinFactor, ordering, values); LinearContainerFactor actFactor(expLinFactor, ordering, values);
LinearContainerFactor actFactorNolin(expLinFactor, ordering);
EXPECT(assert_equal(actFactor, actFactor, tol));
EXPECT(assert_inequal(actFactor, actFactorNolin, tol));
EXPECT(assert_inequal(actFactorNolin, actFactor, tol));
// Check contents // Check contents
Values expLinPoint; Values expLinPoint;