diff --git a/.cproject b/.cproject
index a974f16d4..d2323e19c 100644
--- a/.cproject
+++ b/.cproject
@@ -1538,10 +1538,10 @@
true
true
-
+
make
-j4
- testRegularJacobianFactor.run
+ testRegularHessianFactor.run
true
true
true
diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h
index 8c3df87cf..dbe374958 100644
--- a/gtsam/slam/RegularHessianFactor.h
+++ b/gtsam/slam/RegularHessianFactor.h
@@ -56,13 +56,14 @@ public:
mutable std::vector y;
/** y += alpha * A'*A*x */
- void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
- throw std::runtime_error(
- "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
+ virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+ VectorValues& y) const {
+ HessianFactor::multiplyHessianAdd(alpha, x, y);
}
/** y += alpha * A'*A*x */
- void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
+ void multiplyHessianAdd(double alpha, const double* x,
+ double* yvalues) const {
// Create a vector of temporary y values, corresponding to rows i
y.resize(size());
BOOST_FOREACH(DVector & yi, y)
@@ -95,6 +96,7 @@ public:
}
}
+ /// Raw memory version, with offsets TODO document reasoning
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector offsets) const {
@@ -131,43 +133,38 @@ public:
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
- DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
- alpha * y[i];
+ DMap(yvalues + offsets[keys_[i]],
+ offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
}
/** Return the diagonal of the Hessian for this factor (raw memory version) */
virtual void hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
- //typedef Eigen::Matrix DVector;
typedef Eigen::Matrix DVector;
typedef Eigen::Map DMap;
// Loop over all variables in the factor
- for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+ for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
const Matrix& B = info_(pos, pos).selfadjointView();
- //DMap(d + 9 * j) += B.diagonal();
DMap(d + D * j) += B.diagonal();
}
}
- /* ************************************************************************* */
- // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+ /// Add gradient at zero to d TODO: is it really the goal to add ??
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
- //typedef Eigen::Matrix DVector;
typedef Eigen::Matrix DVector;
typedef Eigen::Map DMap;
// Loop over all variables in the factor
- for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+ for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
- VectorD dj = -info_(pos,size()).knownOffDiagonal();
- //DMap(d + 9 * j) += dj;
+ VectorD dj = -info_(pos, size()).knownOffDiagonal();
DMap(d + D * j) += dj;
}
}
diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h
index 97b2ca460..dcf709854 100644
--- a/gtsam/slam/RegularJacobianFactor.h
+++ b/gtsam/slam/RegularJacobianFactor.h
@@ -57,6 +57,12 @@ public:
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
+ /** y += alpha * A'*A*x */
+ virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+ VectorValues& y) const {
+ JacobianFactor::multiplyHessianAdd(alpha, x, y);
+ }
+
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp
index 8dfb753f4..e2b8ac3cf 100644
--- a/gtsam/slam/tests/testRegularHessianFactor.cpp
+++ b/gtsam/slam/tests/testRegularHessianFactor.cpp
@@ -15,11 +15,10 @@
* @date March 4, 2014
*/
-#include
-#include
-
-//#include
#include
+#include
+
+#include
#include
#include
@@ -29,8 +28,6 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
-const double tol = 1e-5;
-
/* ************************************************************************* */
TEST(RegularHessianFactor, ConstructorNWay)
{
@@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay)
expected.insert(1, Y.segment<2>(2));
expected.insert(3, Y.segment<2>(4));
+ // VectorValues version
+ double alpha = 1.0;
+ VectorValues actualVV;
+ actualVV.insert(0, zero(2));
+ actualVV.insert(1, zero(2));
+ actualVV.insert(3, zero(2));
+ factor.multiplyHessianAdd(alpha, x, actualVV);
+ EXPECT(assert_equal(expected, actualVV));
+
// RAW ACCESS
Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
Vector fast_y = gtsam::zero(8);
double xvalues[8] = {1,2,3,4,0,0,5,6};
- factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+ factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
EXPECT(assert_equal(expected_y, fast_y));
// now, do it with non-zero y
- factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+ factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
EXPECT(assert_equal(2*expected_y, fast_y));
// check some expressions