commit
a76c0a20ee
|
@ -76,7 +76,7 @@ namespace gtsam {
|
|||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
|
@ -86,7 +86,7 @@ namespace gtsam {
|
|||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
|
@ -95,7 +95,7 @@ namespace gtsam {
|
|||
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
|
||||
blockStart_(0)
|
||||
{
|
||||
matrix_.resize(matrix.rows(), matrix.cols());
|
||||
matrix_.setZero(matrix.rows(), matrix.cols());
|
||||
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
|
||||
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
|
||||
if(matrix_.rows() != matrix_.cols())
|
||||
|
|
|
@ -2,10 +2,10 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Line3.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/expressionTesting.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
|
@ -146,7 +146,7 @@ TEST(Line3, projection) {
|
|||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1);
|
||||
Unit3_ projected_(wL_, &Line3::project);
|
||||
ExpressionFactor<Unit3> f(model, expected, projected_);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
|
||||
}
|
||||
|
||||
int main() {
|
||||
|
|
|
@ -342,7 +342,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
|
|||
//******************************************************************************
|
||||
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
|
||||
// Linearization point
|
||||
Vector3 bias; ///< Current estimate of rotation rate bias
|
||||
Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
|
||||
|
||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
|
||||
|
||||
|
|
Loading…
Reference in New Issue