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

Restored VectorValues versions, they work fine

release/4.3a0
dellaert 2015-02-18 10:09:20 +01:00
parent f8f2c2db92
commit 5e568bc29d
4 changed files with 34 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.cproject
View File

@ -1538,10 +1538,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRegularJacobianFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRegularHessianFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRegularJacobianFactor.run</buildTarget>
<buildTarget>testRegularHessianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

27
gtsam/slam/RegularHessianFactor.h
View File

@ -56,13 +56,14 @@ public:
mutable std::vector<DVector> 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<size_t> 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<double, 9, 1> DVector;
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> 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<double, 9, 1> DVector;
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> 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;
}
}

6
gtsam/slam/RegularJacobianFactor.h
View File

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

22
gtsam/slam/tests/testRegularHessianFactor.cpp
View File

@ -15,11 +15,10 @@
* @date March 4, 2014
*/
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
//#include <gtsam_unstable/slam/RegularHessianFactor.h>
#include <gtsam/slam/RegularHessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
@ -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