Merge branch 'feature/BAD' into feature/BAD_generic_value_traits
Conflicts: gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp gtsam_unstable/nonlinear/tests/testExpressionFactor.cpprelease/4.3a0
Mike Bosse
commit
11416cac65
106
.cproject
106
.cproject
|
|
@ -600,7 +600,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>
|
||||
|
|
@ -608,7 +607,6 @@
|
|||
</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>
|
||||
|
|
@ -656,7 +654,6 @@
|
|||
</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>
|
||||
|
|
@ -664,7 +661,6 @@
|
|||
</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>
|
||||
|
|
@ -672,7 +668,6 @@
|
|||
</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>
|
||||
|
|
@ -688,7 +683,6 @@
|
|||
</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>
|
||||
|
|
@ -1120,7 +1114,6 @@
|
|||
</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>
|
||||
|
|
@ -1350,46 +1343,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="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1472,6 +1425,7 @@
|
|||
</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>
|
||||
|
|
@ -1511,6 +1465,7 @@
|
|||
</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>
|
||||
|
|
@ -1518,6 +1473,7 @@
|
|||
</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>
|
||||
|
|
@ -1531,6 +1487,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="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1788,7 +1784,6 @@
|
|||
</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>
|
||||
|
|
@ -1796,7 +1791,6 @@
|
|||
</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>
|
||||
|
|
@ -1804,7 +1798,6 @@
|
|||
</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>
|
||||
|
|
@ -1812,7 +1805,6 @@
|
|||
</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>
|
||||
|
|
@ -2441,6 +2433,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVerticalBlockMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testVerticalBlockMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -2579,7 +2579,6 @@
|
|||
</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>
|
||||
|
|
@ -2587,7 +2586,6 @@
|
|||
</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>
|
||||
|
|
@ -2595,7 +2593,6 @@
|
|||
</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>
|
||||
|
|
@ -3115,6 +3112,7 @@
|
|||
</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>
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
|
|||
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
|
||||
const DenseIndex m = A.rows();
|
||||
if (inf_mask) {
|
||||
// only scale the first v.size() rows of A to support augmented Matrix
|
||||
for (DenseIndex i=0; i<v.size(); ++i) {
|
||||
for (DenseIndex i=0; i<m; ++i) {
|
||||
const double& vi = v(i);
|
||||
if (std::isfinite(vi))
|
||||
A.row(i) *= vi;
|
||||
|
|
|
|||
|
|
@ -416,7 +416,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
|
|||
* Arguments (Matrix, Vector) scales the columns,
|
||||
* (Vector, Matrix) scales the rows
|
||||
* @param inf_mask when true, will not scale with a NaN or inf value.
|
||||
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
|
||||
*/
|
||||
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
|
||||
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
|
||||
|
|
|
|||
|
|
@ -65,9 +65,10 @@ namespace gtsam {
|
|||
|
||||
/** Construct from a container of the sizes of each vertical block. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
|
||||
bool appendOneDimension = false) :
|
||||
variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0) {
|
||||
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
|
|
@ -75,21 +76,23 @@ namespace gtsam {
|
|||
|
||||
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
|
||||
template<typename CONTAINER, typename DERIVED>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
|
||||
{
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions,
|
||||
const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
|
||||
matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
|
||||
rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
|
||||
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
|
||||
if(variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
|
||||
if (variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument(
|
||||
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from iterator over the sizes of each vertical block. */
|
||||
/** Construct from iterator over the sizes of each vertical block. */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
|
||||
DenseIndex height, bool appendOneDimension = false) :
|
||||
variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
|
|
@ -203,18 +206,12 @@ namespace gtsam {
|
|||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
if(appendOneDimension)
|
||||
{
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
friend class SymmetricBlockMatrix;
|
||||
|
|
|
|||
|
|
@ -24,9 +24,20 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using boost::assign::list_of;
|
||||
|
||||
list<size_t> L = list_of(3)(2)(1);
|
||||
vector<size_t> dimensions(L.begin(),L.end());
|
||||
|
||||
//*****************************************************************************
|
||||
TEST(VerticalBlockMatrix, constructor) {
|
||||
VerticalBlockMatrix actual(list_of(3)(2)(1),
|
||||
TEST(VerticalBlockMatrix, Constructor1) {
|
||||
VerticalBlockMatrix actual(dimensions,6);
|
||||
EXPECT_LONGS_EQUAL(6,actual.rows());
|
||||
EXPECT_LONGS_EQUAL(6,actual.cols());
|
||||
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
|
||||
}
|
||||
|
||||
//*****************************************************************************
|
||||
TEST(VerticalBlockMatrix, Constructor2) {
|
||||
VerticalBlockMatrix actual(dimensions,
|
||||
(Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
|
||||
2, 8, 9, 10, 11, 12, //
|
||||
3, 9, 15, 16, 17, 18, //
|
||||
|
|
@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) {
|
|||
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
|
||||
}
|
||||
|
||||
//*****************************************************************************
|
||||
TEST(VerticalBlockMatrix, Constructor3) {
|
||||
VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6);
|
||||
EXPECT_LONGS_EQUAL(6,actual.rows());
|
||||
EXPECT_LONGS_EQUAL(6,actual.cols());
|
||||
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
|
||||
}
|
||||
|
||||
//*****************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
|
|||
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
if (Dpose) {
|
||||
const Matrix R = R_.matrix();
|
||||
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
|
|
@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
|
|||
boost::optional<Matrix3&> Dpoint) const {
|
||||
// Only get transpose once, to avoid multiple allocations,
|
||||
// as well as multiple conversions in the Quaternion case
|
||||
const Matrix3& Rt = R_.transpose();
|
||||
const Matrix3 Rt = R_.transpose();
|
||||
const Point3 q(Rt*(p - t_).vector());
|
||||
if (Dpose) {
|
||||
const double wx = q.x(), wy = q.y(), wz = q.z();
|
||||
|
|
@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
|
|||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
const Matrix3& Rt = R_.transpose();
|
||||
const Matrix3 Rt = R_.transpose();
|
||||
const Point3 q(Rt*(p - t_).vector());
|
||||
if (Dpose) {
|
||||
const double wx = q.x(), wy = q.y(), wz = q.z();
|
||||
|
|
|
|||
|
|
@ -82,20 +82,22 @@ namespace gtsam {
|
|||
class GTSAM_EXPORT JacobianFactor : public GaussianFactor
|
||||
{
|
||||
public:
|
||||
|
||||
typedef JacobianFactor This; ///< Typedef to this class
|
||||
typedef GaussianFactor Base; ///< Typedef to base class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
|
||||
protected:
|
||||
VerticalBlockMatrix Ab_; // the block view of the full matrix
|
||||
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
|
||||
|
||||
public:
|
||||
typedef VerticalBlockMatrix::Block ABlock;
|
||||
typedef VerticalBlockMatrix::constBlock constABlock;
|
||||
typedef ABlock::ColXpr BVector;
|
||||
typedef constABlock::ConstColXpr constBVector;
|
||||
|
||||
protected:
|
||||
|
||||
VerticalBlockMatrix Ab_; // the block view of the full matrix
|
||||
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
|
||||
|
||||
public:
|
||||
|
||||
/** Convert from other GaussianFactor */
|
||||
explicit JacobianFactor(const GaussianFactor& gf);
|
||||
|
|
@ -328,6 +330,21 @@ namespace gtsam {
|
|||
|
||||
private:
|
||||
|
||||
/** Unsafe Constructor that creates an uninitialized Jacobian of right size
|
||||
* @param keys in some order
|
||||
* @param diemnsions of the variables in same order
|
||||
* @param m output dimension
|
||||
* @param model noise model (default NULL)
|
||||
*/
|
||||
template<class KEYS, class DIMENSIONS>
|
||||
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
|
||||
}
|
||||
|
||||
// be very selective on who can access these private methods:
|
||||
template<typename T> friend class ExpressionFactor;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
|
|||
|
|
@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const {
|
|||
// a hard constraint, we don't do anything.
|
||||
const Vector& a = v;
|
||||
const Vector& b = sigmas_;
|
||||
// Now allows for whiten augmented vector with a new additional part coming
|
||||
// from the Lagrange multiplier. So a.size() >= b.size()
|
||||
Vector c = a;
|
||||
for( DenseIndex i = 0; i < b.size(); i++ ) {
|
||||
size_t n = a.size();
|
||||
assert (b.size()==a.size());
|
||||
Vector c(n);
|
||||
for( size_t i = 0; i < n; i++ ) {
|
||||
const double& ai = a(i), &bi = b(i);
|
||||
if (bi!=0) c(i) = ai/bi;
|
||||
c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
|
@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const {
|
|||
/* ************************************************************************* */
|
||||
Matrix Constrained::Whiten(const Matrix& H) const {
|
||||
// selective scaling
|
||||
// Now allow augmented Matrix with a new additional part coming
|
||||
// from the Lagrange multiplier.
|
||||
Matrix M(H.block(0, 0, dim(), H.cols()));
|
||||
Constrained::WhitenInPlace(M);
|
||||
return M;
|
||||
return vector_scale(invsigmas(), H, true);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const {
|
|||
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
|
||||
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
|
||||
// indicating a hard constraint, we leave H's row i in place.
|
||||
// Now allow augmented Matrix with a new additional part coming
|
||||
// from the Lagrange multiplier.
|
||||
// Inlined: vector_scale_inplace(invsigmas(), H, true);
|
||||
// vector_scale_inplace(v, A, true);
|
||||
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
|
||||
|
|
@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
|
||||
Vector sigmas = ones(dim()+augmentedDim);
|
||||
Constrained::shared_ptr Constrained::unit() const {
|
||||
Vector sigmas = ones(dim());
|
||||
for (size_t i=0; i<dim(); ++i)
|
||||
if (this->sigmas_(i) == 0.0)
|
||||
sigmas(i) = 0.0;
|
||||
Vector augmentedMu = zero(dim()+augmentedDim);
|
||||
subInsert(augmentedMu, mu_, 0);
|
||||
return MixedSigmas(augmentedMu, sigmas);
|
||||
return MixedSigmas(mu_, sigmas);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -61,6 +61,11 @@ namespace gtsam {
|
|||
Base(size_t dim = 1):dim_(dim) {}
|
||||
virtual ~Base() {}
|
||||
|
||||
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
|
||||
virtual bool is_constrained() const {
|
||||
return false;
|
||||
}
|
||||
|
||||
/// Dimensionality
|
||||
inline size_t dim() const { return dim_;}
|
||||
|
||||
|
|
@ -385,6 +390,11 @@ namespace gtsam {
|
|||
|
||||
virtual ~Constrained() {}
|
||||
|
||||
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
|
||||
virtual bool is_constrained() const {
|
||||
return true;
|
||||
}
|
||||
|
||||
/// Access mu as a vector
|
||||
const Vector& mu() const { return mu_; }
|
||||
|
||||
|
|
@ -481,9 +491,8 @@ namespace gtsam {
|
|||
/**
|
||||
* Returns a Unit version of a constrained noisemodel in which
|
||||
* constrained sigmas remain constrained and the rest are unit scaled
|
||||
* Now support augmented part from the Lagrange multiplier.
|
||||
*/
|
||||
shared_ptr unit(size_t augmentedDim = 0) const;
|
||||
shared_ptr unit() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -732,7 +741,7 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
|
||||
/// Dipl.-Inform. Jan Oberl<EFBFBD>nder (M.Sc.), FZI Research Center for
|
||||
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
|
||||
/// Information Technology, Karlsruhe, Germany.
|
||||
/// oberlaender@fzi.de
|
||||
/// Thanks Jan!
|
||||
|
|
|
|||
|
|
@ -22,20 +22,21 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
void NonlinearFactor::print(const std::string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << " keys = { ";
|
||||
BOOST_FOREACH(Key key, this->keys()) {
|
||||
BOOST_FOREACH(Key key, keys()) {
|
||||
std::cout << keyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << "}" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
|
||||
return Base::equals(f);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
|
||||
const std::map<Key, Key>& rekey_mapping) const {
|
||||
shared_ptr new_factor = clone();
|
||||
|
|
@ -48,22 +49,23 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
|
|||
return new_factor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
|
||||
const std::vector<Key>& new_keys) const {
|
||||
assert(new_keys.size() == this->keys().size());
|
||||
assert(new_keys.size() == keys().size());
|
||||
shared_ptr new_factor = clone();
|
||||
new_factor->keys() = new_keys;
|
||||
return new_factor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
void NoiseModelFactor::print(const std::string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
|
||||
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
|
||||
return e && Base::equals(f, tol)
|
||||
|
|
@ -72,6 +74,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
|
|||
&& noiseModel_->equals(*e->noiseModel_, tol)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static void check(const SharedNoiseModel& noiseModel, size_t m) {
|
||||
if (!noiseModel)
|
||||
throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
|
||||
|
|
@ -80,14 +83,16 @@ static void check(const SharedNoiseModel& noiseModel, size_t m) {
|
|||
"NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector NoiseModelFactor::whitenedError(const Values& c) const {
|
||||
const Vector b = unwhitenedError(c);
|
||||
check(noiseModel_, b.size());
|
||||
return noiseModel_->whiten(b);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double NoiseModelFactor::error(const Values& c) const {
|
||||
if (this->active(c)) {
|
||||
if (active(c)) {
|
||||
const Vector b = unwhitenedError(c);
|
||||
check(noiseModel_, b.size());
|
||||
return 0.5 * noiseModel_->distance(b);
|
||||
|
|
@ -96,41 +101,36 @@ double NoiseModelFactor::error(const Values& c) const {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
|
||||
const Values& x) const {
|
||||
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(x))
|
||||
if (!active(x))
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
// Call evaluate error to get Jacobians and RHS vector b
|
||||
std::vector<Matrix> A(this->size());
|
||||
std::vector<Matrix> A(size());
|
||||
Vector b = -unwhitenedError(x, A);
|
||||
check(noiseModel_, b.size());
|
||||
|
||||
// Whiten the corresponding system now
|
||||
this->noiseModel_->WhitenSystem(A, b);
|
||||
noiseModel_->WhitenSystem(A, b);
|
||||
|
||||
// Fill in terms, needed to create JacobianFactor below
|
||||
std::vector<std::pair<Key, Matrix> > terms(this->size());
|
||||
for (size_t j = 0; j < this->size(); ++j) {
|
||||
terms[j].first = this->keys()[j];
|
||||
std::vector<std::pair<Key, Matrix> > terms(size());
|
||||
for (size_t j = 0; j < size(); ++j) {
|
||||
terms[j].first = keys()[j];
|
||||
terms[j].second.swap(A[j]);
|
||||
}
|
||||
|
||||
// TODO pass unwhitened + noise model to Gaussian factor
|
||||
// For now, only linearized constrained factors have noise model at linear level!!!
|
||||
noiseModel::Constrained::shared_ptr constrained = //
|
||||
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
|
||||
if (constrained) {
|
||||
// Create a factor of reduced row dimension d_
|
||||
size_t d_ = b.size() - constrained->dim();
|
||||
Vector zero_ = zero(d_);
|
||||
Vector b_ = concatVectors(2, &b, &zero_);
|
||||
noiseModel::Constrained::shared_ptr model = constrained->unit(d_);
|
||||
return boost::make_shared<JacobianFactor>(terms, b_, model);
|
||||
} else
|
||||
return boost::make_shared<JacobianFactor>(terms, b);
|
||||
if (noiseModel_->is_constrained())
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(terms, b,
|
||||
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
|
||||
else
|
||||
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -23,9 +23,11 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
// template meta-programming headers
|
||||
#include <boost/mpl/vector.hpp>
|
||||
|
|
@ -48,7 +50,25 @@ namespace gtsam {
|
|||
template<typename T>
|
||||
class Expression;
|
||||
|
||||
typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap;
|
||||
/**
|
||||
* Expressions are designed to write their derivatives into an already allocated
|
||||
* Jacobian of the correct size, of type VerticalBlockMatrix.
|
||||
* The JacobianMap provides a mapping from keys to the underlying blocks.
|
||||
*/
|
||||
class JacobianMap {
|
||||
const FastVector<Key>& keys_;
|
||||
VerticalBlockMatrix& Ab_;
|
||||
public:
|
||||
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
|
||||
keys_(keys), Ab_(Ab) {
|
||||
}
|
||||
/// Access via key
|
||||
VerticalBlockMatrix::Block operator()(Key key) {
|
||||
FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
|
||||
DenseIndex block = it - keys_.begin();
|
||||
return Ab_(block);
|
||||
}
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
/**
|
||||
|
|
@ -78,16 +98,14 @@ struct CallRecord {
|
|||
template<int ROWS, int COLS>
|
||||
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
|
||||
JacobianMap& jacobians, Key key) {
|
||||
JacobianMap::iterator it = jacobians.find(key);
|
||||
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
|
||||
jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
|
||||
}
|
||||
/// Handle Leaf Case for Dynamic Matrix type (slower)
|
||||
template<>
|
||||
void handleLeafCase(
|
||||
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
|
||||
JacobianMap& jacobians, Key key) {
|
||||
JacobianMap::iterator it = jacobians.find(key);
|
||||
it->second += dTdA;
|
||||
jacobians(key) += dTdA;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -123,7 +123,7 @@ public:
|
|||
}
|
||||
|
||||
/// Return value and derivatives, reverse AD version
|
||||
T reverse(const Values& values, JacobianMap& jacobians) const {
|
||||
T value(const Values& values, JacobianMap& jacobians) const {
|
||||
// The following piece of code is absolutely crucial for performance.
|
||||
// We allocate a block of memory on the stack, which can be done at runtime
|
||||
// with modern C++ compilers. The traceExecution then fills this memory
|
||||
|
|
@ -142,11 +142,6 @@ public:
|
|||
return root_->value(values);
|
||||
}
|
||||
|
||||
/// Return value and derivatives
|
||||
T value(const Values& values, JacobianMap& jacobians) const {
|
||||
return reverse(values, jacobians);
|
||||
}
|
||||
|
||||
const boost::shared_ptr<ExpressionNode<T> >& root() const {
|
||||
return root_;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
|
||||
* Error function *without* the NoiseModel, \f$ h(x)-z \f$.
|
||||
* We override this method to provide
|
||||
* both the function evaluation and its derivative(s) in H.
|
||||
*/
|
||||
|
|
@ -85,17 +85,19 @@ public:
|
|||
// H should be pre-allocated
|
||||
assert(H->size()==size());
|
||||
|
||||
// Create and zero out blocks to be passed to expression_
|
||||
JacobianMap blocks;
|
||||
for (DenseIndex i = 0; i < size(); i++) {
|
||||
Matrix& Hi = H->at(i);
|
||||
Hi.resize(Dim, dimensions_[i]);
|
||||
Hi.setZero(); // zero out
|
||||
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
|
||||
blocks.insert(std::make_pair(keys_[i], block));
|
||||
}
|
||||
VerticalBlockMatrix Ab(dimensions_, Dim);
|
||||
|
||||
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
|
||||
JacobianMap map(keys_, Ab);
|
||||
Ab.matrix().setZero();
|
||||
|
||||
// Evaluate error to get Jacobians and RHS vector b
|
||||
T value = expression_.value(x, map); // <<< Reverse AD happens here !
|
||||
|
||||
// Copy blocks into the vector of jacobians passed in
|
||||
for (DenseIndex i = 0; i < size(); i++)
|
||||
H->at(i) = Ab(i);
|
||||
|
||||
T value = expression_.value(x, blocks);
|
||||
return measurement_.localCoordinates(value);
|
||||
} else {
|
||||
const T& value = expression_.value(x);
|
||||
|
|
@ -105,41 +107,34 @@ public:
|
|||
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
|
||||
|
||||
// This method has been heavily optimized for maximum performance.
|
||||
// We allocate a VerticalBlockMatrix on the stack first, and then create
|
||||
// Eigen::Block<Matrix> views on this piece of memory which is then passed
|
||||
// to [expression_.value] below, which writes directly into Ab_.
|
||||
// Only linearize if the factor is active
|
||||
if (!active(x))
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
// Another malloc saved by creating a Matrix on the stack
|
||||
double memory[Dim * augmentedCols_];
|
||||
Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
|
||||
matrix(memory, Dim, augmentedCols_);
|
||||
matrix.setZero(); // zero out
|
||||
// Create a writeable JacobianFactor in advance
|
||||
// In case noise model is constrained, we need to provide a noise model
|
||||
bool constrained = noiseModel_->is_constrained();
|
||||
boost::shared_ptr<JacobianFactor> factor(
|
||||
constrained ? new JacobianFactor(keys_, dimensions_, Dim,
|
||||
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
|
||||
new JacobianFactor(keys_, dimensions_, Dim));
|
||||
|
||||
// Construct block matrix, is of right size but un-initialized
|
||||
VerticalBlockMatrix Ab(dimensions_, matrix, true);
|
||||
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
|
||||
VerticalBlockMatrix& Ab = factor->matrixObject();
|
||||
JacobianMap map(keys_, Ab);
|
||||
|
||||
// Create blocks into Ab_ to be passed to expression_
|
||||
JacobianMap blocks;
|
||||
for (DenseIndex i = 0; i < size(); i++)
|
||||
blocks.insert(std::make_pair(keys_[i], Ab(i)));
|
||||
// Zero out Jacobian so we can simply add to it
|
||||
Ab.matrix().setZero();
|
||||
|
||||
// Evaluate error to get Jacobians and RHS vector b
|
||||
T value = expression_.value(x, blocks); // <<< Reverse AD happens here !
|
||||
T value = expression_.value(x, map); // <<< Reverse AD happens here !
|
||||
Ab(size()).col(0) = -measurement_.localCoordinates(value);
|
||||
|
||||
// Whiten the corresponding system now
|
||||
// TODO ! this->noiseModel_->WhitenSystem(Ab);
|
||||
// Whiten the corresponding system, Ab already contains RHS
|
||||
Vector dummy(Dim);
|
||||
noiseModel_->WhitenSystem(Ab.matrix(),dummy);
|
||||
|
||||
// TODO pass unwhitened + noise model to Gaussian factor
|
||||
// For now, only linearized constrained factors have noise model at linear level!!!
|
||||
noiseModel::Constrained::shared_ptr constrained = //
|
||||
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
|
||||
if (constrained) {
|
||||
return boost::make_shared<JacobianFactor>(this->keys(), Ab,
|
||||
constrained->unit());
|
||||
} else
|
||||
return boost::make_shared<JacobianFactor>(this->keys(), Ab);
|
||||
return factor;
|
||||
}
|
||||
};
|
||||
// ExpressionFactor
|
||||
|
|
|
|||
|
|
@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
|
|||
TEST(Expression, constant) {
|
||||
Expression<Rot3> R(someR);
|
||||
Values values;
|
||||
JacobianMap actualMap;
|
||||
Rot3 actual = R.value(values, actualMap);
|
||||
Rot3 actual = R.value(values);
|
||||
EXPECT(assert_equal(someR, actual));
|
||||
JacobianMap expected;
|
||||
EXPECT(actualMap == expected);
|
||||
EXPECT_LONGS_EQUAL(0, R.traceSize())
|
||||
}
|
||||
|
||||
|
|
@ -61,15 +58,8 @@ TEST(Expression, Leaf) {
|
|||
Values values;
|
||||
values.insert(100, someR);
|
||||
|
||||
JacobianMap expected;
|
||||
Matrix H = eye(3);
|
||||
expected.insert(make_pair(100, H.block(0, 0, 3, 3)));
|
||||
|
||||
JacobianMap actualMap2;
|
||||
actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3)));
|
||||
Rot3 actual2 = R.reverse(values, actualMap2);
|
||||
Rot3 actual2 = R.value(values);
|
||||
EXPECT(assert_equal(someR, actual2));
|
||||
EXPECT(actualMap2 == expected);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -28,6 +28,9 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
using boost::assign::list_of;
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
|
@ -62,41 +65,41 @@ TEST(ExpressionFactor, Leaf) {
|
|||
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//// non-zero noise model
|
||||
//TEST(ExpressionFactor, Model) {
|
||||
// using namespace leaf;
|
||||
//
|
||||
// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
|
||||
//
|
||||
// // Create old-style factor to create expected value and derivatives
|
||||
// PriorFactor<Point2> old(2, Point2(0, 0), model);
|
||||
//
|
||||
// // Concise version
|
||||
// ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
// EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// Constrained noise model
|
||||
//TEST(ExpressionFactor, Constrained) {
|
||||
// using namespace leaf;
|
||||
//
|
||||
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
|
||||
//
|
||||
// // Create old-style factor to create expected value and derivatives
|
||||
// PriorFactor<Point2> old(2, Point2(0, 0), model);
|
||||
//
|
||||
// // Concise version
|
||||
// ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
// EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
//}
|
||||
/* ************************************************************************* */
|
||||
// non-zero noise model
|
||||
TEST(ExpressionFactor, Model) {
|
||||
using namespace leaf;
|
||||
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
|
||||
|
||||
// Create old-style factor to create expected value and derivatives
|
||||
PriorFactor<Point2> old(2, Point2(0, 0), model);
|
||||
|
||||
// Concise version
|
||||
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Constrained noise model
|
||||
TEST(ExpressionFactor, Constrained) {
|
||||
using namespace leaf;
|
||||
|
||||
SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
|
||||
|
||||
// Create old-style factor to create expected value and derivatives
|
||||
PriorFactor<Point2> old(2, Point2(0, 0), model);
|
||||
|
||||
// Concise version
|
||||
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Unary(Leaf))
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define time timeMultiThreaded
|
||||
#define time timeSingleThreaded
|
||||
|
||||
boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
|
||||
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define time timeMultiThreaded
|
||||
#define time timeSingleThreaded
|
||||
|
||||
int main() {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue