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

Switched to new Eigen built-in special comma initializer

release/4.3a0
Richard Roberts 2013-12-16 21:33:12 +00:00
parent b88dcfe32b
commit 880d9a8e3c
125 changed files with 1196 additions and 1415 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CameraResectioning.cpp
View File

@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vec(2) << 0.5, 0.5));
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

8
examples/LocalizationExample.cpp
View File

@ -93,8 +93,8 @@ public:
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Mat(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vec(2) << q.x() - mx_, q.y() - my_);
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vector(2) << q.x() - mx_, q.y() - my_);
}
// The second is a 'clone' function that allows the factor to be copied. Under most
@ -118,14 +118,14 @@ int main(int argc, char** argv) {
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); // 10cm std on x,y
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

4
examples/OdometryExample.cpp
View File

@ -64,13 +64,13 @@ int main(int argc, char** argv) {
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

6
examples/PlanarSLAMExample.cpp
View File

@ -80,18 +80,18 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),

4
examples/Pose2SLAMExample.cpp
View File

@ -71,11 +71,11 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses

4
examples/Pose2SLAMExample_graph.cpp
View File

@ -31,13 +31,13 @@ int main (int argc, char** argv) {
// we are in build/examples, data is in examples/Data
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.01, 0.01, 0.01));
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -32,11 +32,11 @@ int main (int argc, char** argv) {
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));

6
examples/Pose2SLAMwSPCG.cpp
View File

@ -68,12 +68,12 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
@ -85,7 +85,7 @@ int main(int argc, char** argv) {
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print

2
examples/SFMExample.cpp
View File

@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph

4
examples/SelfCalibrationExample.cpp
View File

@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vec(5) << 500, 500, 0.1, 100, 100));
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution

2
examples/VisualISAM2Example.cpp
View File

@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0

2
examples/VisualISAMExample.cpp
View File

@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0

8
examples/easyPoint2KalmanFilter.cpp
View File

@ -37,7 +37,7 @@ int main() {
// Create the Kalman Filter initialization point
Point2 x_initial(0.0, 0.0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
// Create Key for initial pose
Symbol x0('x',0);
@ -57,8 +57,8 @@ int main() {
// For the purposes of this example, let us assume we are using a constant-position model and
// the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
// and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
Vector u = (Vec(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1), true);
Vector u = (Vector(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
// This simple motion can be modeled with a BetweenFactor
// Create Key for next pose
@ -83,7 +83,7 @@ int main() {
// For the purposes of this example, let us assume we have something like a GPS that returns
// the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
// R = [0.25 0 ; 0 0.25].
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25), true);
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true);
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);

4
gtsam/base/LieScalar.h
View File

@ -58,7 +58,7 @@ namespace gtsam {
LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
/** @return the local coordinates of another object */
Vector localCoordinates(const LieScalar& t2) const { return (Vec(1) << (t2.value() - value())); }
Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())); }
// Group requirements
@ -96,7 +96,7 @@ namespace gtsam {
static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
/** Logmap around identity - just returns with default cast back */
static Vector Logmap(const LieScalar& p) { return (Vec(1) << p.value()); }
static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()); }
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {

2
gtsam/base/LieVector.h
View File

@ -39,7 +39,7 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
/** wrap a double */
LieVector(double d) : Vector((Vec(1) << d)) {}
LieVector(double d) : Vector((Vector(1) << d)) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);

218
gtsam/base/SpecialCommaInitializer.h
View File

@ -1,218 +0,0 @@
/**
* @file SpecialCommaInitializer.h
* @brief A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix.
* @author Richard Roberts
* @created Oct 10, 2013
*/
#pragma once
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace Eigen {
namespace internal {
// Row-vectors not tested
//template<typename XprType>
//inline void resizeHelper(XprType& xpr, DenseIndex sizeIncrement,
// typename boost::enable_if_c<
// XprType::ColsAtCompileTime == Dynamic && XprType::RowsAtCompileTime == 1>::type* = 0)
//{
// xpr.conservativeResize(xpr.cols() + sizeIncrement);
//}
template<typename XprType>
inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement,
typename boost::enable_if_c<
XprType::RowsAtCompileTime == Dynamic && XprType::ColsAtCompileTime == 1>::type* = 0)
{
xpr.conservativeResize(xpr.rows() + sizeIncrement);
}
template<typename XprType>
inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement,
typename boost::enable_if_c<
XprType::ColsAtCompileTime == Dynamic>::type* = 0)
{
assert(false);
}
}
/// A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix.
template<typename XprType>
class SpecialCommaInitializer :
public CommaInitializer<XprType>,
public MatrixBase<SpecialCommaInitializer<XprType> >
{
private:
bool dynamic_;
public:
typedef MatrixBase<SpecialCommaInitializer<XprType> > Base;
typedef CommaInitializer<XprType> CommaBase;
EIGEN_DENSE_PUBLIC_INTERFACE(SpecialCommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
// Forward to base class
inline SpecialCommaInitializer(XprType& xpr, const typename XprType::Scalar& s, bool dynamic) :
CommaBase(xpr, s), dynamic_(dynamic) {}
// Forward to base class
template<typename OtherDerived>
inline SpecialCommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other, bool dynamic) :
CommaBase(xpr, other), dynamic_(dynamic) {}
inline Index rows() const { return CommaBase::m_xpr.rows(); }
inline Index cols() const { return CommaBase::m_xpr.cols(); }
inline Index outerStride() const { return CommaBase::m_xpr.outerStride(); }
inline Index innerStride() const { return CommaBase::m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const
{
return CommaBase::m_xpr.coeff(row, col);
}
inline CoeffReturnType coeff(Index index) const
{
return CommaBase::m_xpr.coeff(index);
}
inline const Scalar& coeffRef(Index row, Index col) const
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col);
}
inline const Scalar& coeffRef(Index index) const
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(index);
}
inline Scalar& coeffRef(Index row, Index col)
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(Index index)
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return CommaBase::m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
CommaBase::m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return CommaBase::m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
CommaBase::m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
}
const XprType& _expression() const { return CommaBase::m_xpr; }
/// Override base class comma operators to return this class instead of the base class.
SpecialCommaInitializer& operator,(const typename XprType::Scalar& s)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Dynamic);
// Current col should be zero and row should be at the end
assert(CommaBase::m_col == 1);
assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows);
resizeHelper(CommaBase::m_xpr, 1);
}
(void) CommaBase::operator,(s);
return *this;
}
/// Override base class comma operators to return this class instead of the base class.
template<typename OtherDerived>
SpecialCommaInitializer& operator,(const DenseBase<OtherDerived>& other)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Dynamic);
// Current col should be zero and row should be at the end
assert(CommaBase::m_col == 1);
assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows);
resizeHelper(CommaBase::m_xpr, other.size());
}
(void) CommaBase::operator,(other);
return *this;
}
};
namespace internal {
template<typename ExpressionType>
struct traits<SpecialCommaInitializer<ExpressionType> > : traits<ExpressionType>
{
};
}
}
namespace gtsam {
class Vec
{
Eigen::VectorXd vector_;
bool dynamic_;
public:
Vec(Eigen::VectorXd::Index size) : vector_(size), dynamic_(false) {}
Vec() : dynamic_(true) {}
Eigen::SpecialCommaInitializer<Eigen::VectorXd> operator<< (double s)
{
if(dynamic_)
vector_.resize(1);
return Eigen::SpecialCommaInitializer<Eigen::VectorXd>(vector_, s, dynamic_);
}
template<typename OtherDerived>
Eigen::SpecialCommaInitializer<Eigen::VectorXd> operator<<(const Eigen::DenseBase<OtherDerived>& other)
{
if(dynamic_)
vector_.resize(other.size());
return Eigen::SpecialCommaInitializer<Eigen::VectorXd>(vector_, other, dynamic_);
}
};
class Mat
{
Eigen::MatrixXd matrix_;
public:
Mat(Eigen::MatrixXd::Index rows, Eigen::MatrixXd::Index cols) : matrix_(rows, cols) {}
Eigen::SpecialCommaInitializer<Eigen::MatrixXd> operator<< (double s)
{
return Eigen::SpecialCommaInitializer<Eigen::MatrixXd>(matrix_, s, false);
}
template<typename OtherDerived>
Eigen::SpecialCommaInitializer<Eigen::MatrixXd> operator<<(const Eigen::DenseBase<OtherDerived>& other)
{
return Eigen::SpecialCommaInitializer<Eigen::MatrixXd>(matrix_, other, false);
}
};
}

1
gtsam/base/Vector.h
View File

@ -25,7 +25,6 @@
#include <iostream>
#include <gtsam/global_includes.h>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam/base/SpecialCommaInitializer.h>
namespace gtsam {

6
gtsam/base/tests/testCholesky.cpp
View File

@ -27,7 +27,7 @@ TEST(cholesky, choleskyPartial) {
// choleskyPartial should only use the upper triangle, so this represents a
// symmetric matrix.
Matrix ABC = (Mat(7,7) <<
Matrix ABC = (Matrix(7,7) <<
4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063,
0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914,
0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992,
@ -55,7 +55,7 @@ TEST(cholesky, choleskyPartial) {
/* ************************************************************************* */
TEST(cholesky, BadScalingCholesky) {
Matrix A = (Mat(2,2) <<
Matrix A = (Matrix(2,2) <<
1e-40, 0.0,
0.0, 1.0);
@ -70,7 +70,7 @@ TEST(cholesky, BadScalingCholesky) {
/* ************************************************************************* */
TEST(cholesky, BadScalingSVD) {
Matrix A = (Mat(2,2) <<
Matrix A = (Matrix(2,2) <<
1.0, 0.0,
0.0, 1e-40);

6
gtsam/base/tests/testLieMatrix.cpp
View File

@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix)
/* ************************************************************************* */
TEST( LieMatrix, construction ) {
Matrix m = (Mat(2,2) << 1.0,2.0, 3.0,4.0);
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0);
LieMatrix lie1(m), lie2(m);
EXPECT(lie1.dim() == 4);
@ -37,7 +37,7 @@ TEST( LieMatrix, construction ) {
/* ************************************************************************* */
TEST( LieMatrix, other_constructors ) {
Matrix init = (Mat(2,2) << 10.0,20.0, 30.0,40.0);
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
LieMatrix exp(init);
LieMatrix a(2,2,10.0,20.0,30.0,40.0);
double data[] = {10,30,20,40};
@ -62,7 +62,7 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vec() << 1, 2, 3, 4);
Vector expectedLogmap = (Vector() << 1, 2, 3, 4);
Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}

4
gtsam/base/tests/testLieVector.cpp
View File

@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieVector)
/* ************************************************************************* */
TEST( testLieVector, construction ) {
Vector v = (Vec(3) << 1.0, 2.0, 3.0);
Vector v = (Vector(3) << 1.0, 2.0, 3.0);
LieVector lie1(v), lie2(v);
EXPECT(lie1.dim() == 3);
@ -37,7 +37,7 @@ TEST( testLieVector, construction ) {
/* ************************************************************************* */
TEST( testLieVector, other_constructors ) {
Vector init = (Vec(2) << 10.0, 20.0);
Vector init = (Vector(2) << 10.0, 20.0);
LieVector exp(init);
LieVector a(2,10.0,20.0);
double data[] = {10,20};

240
gtsam/base/tests/testMatrix.cpp
View File

@ -32,7 +32,7 @@ static const double tol = 1e-9;
/* ************************************************************************* */
TEST( matrix, constructor_data )
{
Matrix A = (Mat(2, 2) << -5, 3, 0, -5);
Matrix A = (Matrix(2, 2) << -5, 3, 0, -5);
Matrix B(2, 2);
B(0, 0) = -5;
@ -58,7 +58,7 @@ TEST( matrix, constructor_vector )
/* ************************************************************************* */
TEST( matrix, Matrix_ )
{
Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B(2, 2);
B(0, 0) = -5;
B(0, 1) = 3;
@ -94,17 +94,17 @@ TEST( matrix, special_comma_initializer)
expected(1,0) = 3;
expected(1,1) = 4;
Matrix actual1 = (Mat(2,2) << 1, 2, 3, 4);
Matrix actual2((Mat(2,2) << 1, 2, 3, 4));
Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4);
Matrix actual2((Matrix(2,2) << 1, 2, 3, 4));
Matrix submat1 = (Mat(1,2) << 3, 4);
Matrix actual3 = (Mat(2,2) << 1, 2, submat1);
Matrix submat1 = (Matrix(1,2) << 3, 4);
Matrix actual3 = (Matrix(2,2) << 1, 2, submat1);
Matrix submat2 = (Mat(1,2) << 1, 2);
Matrix actual4 = (Mat(2,2) << submat2, 3, 4);
Matrix submat2 = (Matrix(1,2) << 1, 2);
Matrix actual4 = (Matrix(2,2) << submat2, 3, 4);
Matrix actual5 = testFcn1((Mat(2,2) << 1, 2, 3, 4));
Matrix actual6 = testFcn2((Mat(2,2) << 1, 2, 3, 4));
Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4));
Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4));
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
@ -117,7 +117,7 @@ TEST( matrix, special_comma_initializer)
/* ************************************************************************* */
TEST( matrix, col_major )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
const double * const a = &A(0, 0);
EXPECT_DOUBLES_EQUAL(1, a[0], tol);
EXPECT_DOUBLES_EQUAL(3, a[1], tol);
@ -128,8 +128,8 @@ TEST( matrix, col_major )
/* ************************************************************************* */
TEST( matrix, collect1 )
{
Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Mat(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
Matrix AB = collect(2, &A, &B);
Matrix C(2, 5);
for (int i = 0; i < 2; i++)
@ -145,8 +145,8 @@ TEST( matrix, collect1 )
/* ************************************************************************* */
TEST( matrix, collect2 )
{
Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Mat(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
vector<const Matrix*> matrices;
matrices.push_back(&A);
matrices.push_back(&B);
@ -172,7 +172,7 @@ TEST( matrix, collect3 )
matrices.push_back(&A);
matrices.push_back(&B);
Matrix AB = collect(matrices, 2, 3);
Matrix exp = (Mat(2, 6) <<
Matrix exp = (Matrix(2, 6) <<
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0);
@ -182,8 +182,8 @@ TEST( matrix, collect3 )
/* ************************************************************************* */
TEST( matrix, stack )
{
Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Mat(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0);
Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
Matrix AB = stack(2, &A, &B);
Matrix C(5, 2);
for (int i = 0; i < 2; i++)
@ -205,19 +205,19 @@ TEST( matrix, stack )
/* ************************************************************************* */
TEST( matrix, column )
{
Matrix A = (Mat(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
-0.1);
Vector a1 = column(A, 0);
Vector exp1 = (Vec(4) << -1., 0., 1., 0.);
Vector exp1 = (Vector(4) << -1., 0., 1., 0.);
EXPECT(assert_equal(a1, exp1));
Vector a2 = column(A, 3);
Vector exp2 = (Vec(4) << 0., 1., 0., 0.);
Vector exp2 = (Vector(4) << 0., 1., 0., 0.);
EXPECT(assert_equal(a2, exp2));
Vector a3 = column(A, 6);
Vector exp3 = (Vec(4) << -0.2, 0.3, 0.2, -0.1);
Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1);
EXPECT(assert_equal(a3, exp3));
}
@ -230,7 +230,7 @@ TEST( matrix, insert_column )
insertColumn(big, col, j);
Matrix expected = (Mat(5, 6) <<
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -253,7 +253,7 @@ TEST( matrix, insert_subcolumn )
Vector col2 = ones(1);
insertColumn(big, col2, 4, 5); // check 2
Matrix expected = (Mat(5, 6) <<
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -266,19 +266,19 @@ TEST( matrix, insert_subcolumn )
/* ************************************************************************* */
TEST( matrix, row )
{
Matrix A = (Mat(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
-0.1);
Vector a1 = row(A, 0);
Vector exp1 = (Vec(7) << -1., 0., 1., 0., 0., 0., -0.2);
Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2);
EXPECT(assert_equal(a1, exp1));
Vector a2 = row(A, 2);
Vector exp2 = (Vec(7) << 1., 0., 0., 0., -1., 0., 0.2);
Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2);
EXPECT(assert_equal(a2, exp2));
Vector a3 = row(A, 3);
Vector exp3 = (Vec(7) << 0., 1., 0., 0., 0., -1., -0.1);
Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1);
EXPECT(assert_equal(a3, exp3));
}
@ -301,12 +301,12 @@ TEST( matrix, zeros )
/* ************************************************************************* */
TEST( matrix, insert_sub )
{
Matrix big = zeros(5, 6), small = (Mat(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0);
insertSub(big, small, 1, 2);
Matrix expected = (Mat(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
@ -323,7 +323,7 @@ TEST( matrix, diagMatrices )
Matrix actual = diag(Hs);
Matrix expected = (Mat(9, 9) <<
Matrix expected = (Matrix(9, 9) <<
1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@ -339,7 +339,7 @@ TEST( matrix, diagMatrices )
/* ************************************************************************* */
TEST( matrix, stream_read ) {
Matrix expected = (Mat(3,4) <<
Matrix expected = (Matrix(3,4) <<
1.1, 2.3, 4.2, 7.6,
-0.3, -8e-2, 5.1, 9.0,
1.2, 3.4, 4.5, 6.7);
@ -374,7 +374,7 @@ TEST( matrix, scale_columns )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vec(4) << 2., 3., 4., 5.);
Vector v = (Vector(4) << 2., 3., 4., 5.);
Matrix actual = vector_scale(A, v);
@ -412,7 +412,7 @@ TEST( matrix, scale_rows )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vec(3) << 2., 3., 4.);
Vector v = (Vector(3) << 2., 3., 4.);
Matrix actual = vector_scale(v, A);
@ -450,7 +450,7 @@ TEST( matrix, scale_rows_mask )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vec(3) << 2., std::numeric_limits<double>::infinity(), 4.);
Vector v = (Vector(3) << 2., std::numeric_limits<double>::infinity(), 4.);
Matrix actual = vector_scale(v, A, true);
@ -533,18 +533,18 @@ TEST( matrix, equal_nan )
/* ************************************************************************* */
TEST( matrix, addition )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Mat(2, 2) << 5.0, 5.0, 5.0, 5.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0);
EQUALITY(A+B,C);
}
/* ************************************************************************* */
TEST( matrix, addition_in_place )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Mat(2, 2) << 5.0, 5.0, 5.0, 5.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0);
A += B;
EQUALITY(A,C);
}
@ -552,18 +552,18 @@ TEST( matrix, addition_in_place )
/* ************************************************************************* */
TEST( matrix, subtraction )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Mat(2, 2) << -3.0, -1.0, 1.0, 3.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0);
EQUALITY(A-B,C);
}
/* ************************************************************************* */
TEST( matrix, subtraction_in_place )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Mat(2, 2) << -3.0, -1.0, 1.0, 3.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0);
Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0);
A -= B;
EQUALITY(A,C);
}
@ -613,10 +613,10 @@ TEST( matrix, matrix_vector_multiplication )
{
Vector result(2);
Matrix A = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Vector v = (Vec(3) << 1., 2., 3.);
Vector Av = (Vec(2) << 14., 32.);
Vector AtAv = (Vec(3) << 142., 188., 234.);
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Vector v = (Vector(3) << 1., 2., 3.);
Vector Av = (Vector(2) << 14., 32.);
Vector AtAv = (Vector(3) << 142., 188., 234.);
EQUALITY(A*v,Av);
EQUALITY(A^Av,AtAv);
@ -650,12 +650,12 @@ TEST( matrix, scalar_divide )
/* ************************************************************************* */
TEST( matrix, zero_below_diagonal ) {
Matrix A1 = (Mat(3, 4) <<
Matrix A1 = (Matrix(3, 4) <<
1.0, 2.0, 3.0, 4.0,
1.0, 2.0, 3.0, 4.0,
1.0, 2.0, 3.0, 4.0);
Matrix expected1 = (Mat(3, 4) <<
Matrix expected1 = (Matrix(3, 4) <<
1.0, 2.0, 3.0, 4.0,
0.0, 2.0, 3.0, 4.0,
0.0, 0.0, 3.0, 4.0);
@ -671,13 +671,13 @@ TEST( matrix, zero_below_diagonal ) {
zeroBelowDiagonal(actual1c, 4);
EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10));
Matrix A2 = (Mat(5, 3) <<
Matrix A2 = (Matrix(5, 3) <<
1.0, 2.0, 3.0,
1.0, 2.0, 3.0,
1.0, 2.0, 3.0,
1.0, 2.0, 3.0,
1.0, 2.0, 3.0);
Matrix expected2 = (Mat(5, 3) <<
Matrix expected2 = (Matrix(5, 3) <<
1.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 0.0, 3.0,
@ -692,7 +692,7 @@ TEST( matrix, zero_below_diagonal ) {
zeroBelowDiagonal(actual2c);
EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10));
Matrix expected2_partial = (Mat(5, 3) <<
Matrix expected2_partial = (Matrix(5, 3) <<
1.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0,
@ -735,14 +735,14 @@ TEST( matrix, inverse )
EXPECT(assert_equal(expected, Ainv, 1e-4));
// These two matrices failed before version 2003 because we called LU incorrectly
Matrix lMg((Mat(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0));
EXPECT(assert_equal((Mat(3, 3) <<
Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0));
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
inverse(lMg)));
Matrix gMl((Mat(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0));
EXPECT(assert_equal((Mat(3, 3) <<
Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0));
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
@ -783,19 +783,19 @@ TEST( matrix, inverse2 )
TEST( matrix, backsubtitution )
{
// TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = (Vec(2) << 3.6250, -0.75);
Matrix U22 = (Mat(2, 2) << 2., 3., 0., 4.);
Vector expected1 = (Vector(2) << 3.6250, -0.75);
Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.);
Vector b1 = U22 * expected1;
EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
// TEST TWO 3x3 matrix U2*x=b2
Vector expected2 = (Vec(3) << 5.5, -8.5, 5.);
Matrix U33 = (Mat(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.);
Vector expected2 = (Vector(3) << 5.5, -8.5, 5.);
Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.);
Vector b2 = U33 * expected2;
EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
// TEST THREE Lower triangular 3x3 matrix L3*x=b3
Vector expected3 = (Vec(3) << 1., 1., 1.);
Vector expected3 = (Vector(3) << 1., 1., 1.);
Matrix L3 = trans(U33);
Vector b3 = L3 * expected3;
EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
@ -809,11 +809,11 @@ TEST( matrix, householder )
{
// check in-place householder, with v vectors below diagonal
Matrix expected1 = (Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
Matrix expected1 = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
-0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894);
Matrix A1 = (Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
Matrix A1 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1 );
@ -822,10 +822,10 @@ TEST( matrix, householder )
// in-place, with zeros below diagonal
Matrix expected = (Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
Matrix expected = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
0, 0, 4.4721, 0, -4.4721, 0.894);
Matrix A2 = (Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
Matrix A2 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1);
@ -838,11 +838,11 @@ TEST( matrix, householder_colMajor )
{
// check in-place householder, with v vectors below diagonal
Matrix expected1((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
Matrix expected1((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
-0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894));
Matrix A1((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
Matrix A1((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1));
@ -851,10 +851,10 @@ TEST( matrix, householder_colMajor )
// in-place, with zeros below diagonal
Matrix expected((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
0, 0, 4.4721, 0, -4.4721, 0.894));
Matrix A2((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
Matrix A2((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1));
@ -869,10 +869,10 @@ TEST( matrix, eigen_QR )
// in-place, with zeros below diagonal
Matrix expected((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
0, 0, 4.4721, 0, -4.4721, 0.894));
Matrix A((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
Matrix A((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1));
@ -882,7 +882,7 @@ TEST( matrix, eigen_QR )
EXPECT(assert_equal(expected, actual, 1e-3));
// use shiny new in place QR inside gtsam
A = Matrix((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1,
A = Matrix((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
00,-5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10,0, 2,
00, 10,0, 0, 0, -10, -1));
@ -897,16 +897,16 @@ TEST( matrix, eigen_QR )
TEST( matrix, qr )
{
Matrix A = (Mat(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
0, 0, -10, 10, 0, -10, 0);
Matrix expectedQ = (Mat(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0,
Matrix expectedQ = (Matrix(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0,
0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944,
0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667,
0, -0.5963, 0, 0, -0.4472);
Matrix expectedR = (Mat(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
Matrix expectedR = (Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0);
Matrix Q, R;
@ -919,11 +919,11 @@ TEST( matrix, qr )
/* ************************************************************************* */
TEST( matrix, sub )
{
Matrix A = (Mat(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
0, 00, 10, 0, 0, 0, -10);
Matrix actual = sub(A, 1, 3, 1, 5);
Matrix expected = (Mat(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10);
Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10);
EQUALITY(actual,expected);
}
@ -931,15 +931,15 @@ TEST( matrix, sub )
/* ************************************************************************* */
TEST( matrix, trans )
{
Matrix A = (Mat(2, 2) << 1.0, 3.0, 2.0, 4.0);
Matrix B = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0);
Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
EQUALITY(trans(A),B);
}
/* ************************************************************************* */
TEST( matrix, col_major_access )
{
Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
const double* a = &A(0, 0);
DOUBLES_EQUAL(2.0,a[2],1e-9);
}
@ -948,16 +948,16 @@ TEST( matrix, col_major_access )
TEST( matrix, weighted_elimination )
{
// create a matrix to eliminate
Matrix A = (Mat(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.);
Vector b = (Vec(4) << -0.2, 0.3, 0.2, -0.1);
Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1);
Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1);
Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1);
// expected values
Matrix expectedR = (Mat(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0.,
Matrix expectedR = (Matrix(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0.,
-0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.);
Vector d = (Vec(4) << 0.2, -0.14, 0.0, 0.2);
Vector newSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2);
Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
Vector r;
double di, sigma;
@ -982,11 +982,11 @@ TEST( matrix, weighted_elimination )
/* ************************************************************************* */
TEST( matrix, inverse_square_root )
{
Matrix measurement_covariance = (Mat(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
0.0, 0.0, 0.0, 0.01);
Matrix actual = inverse_square_root(measurement_covariance);
Matrix expected = (Mat(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
10.0);
EQUALITY(expected,actual);
@ -998,14 +998,14 @@ TEST( matrix, inverse_square_root )
// use the same inverse routing inside inverse_square_root()
// as we use here to check it.
Matrix M = (Mat(5, 5) <<
Matrix M = (Matrix(5, 5) <<
0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726,
0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868,
-0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064,
-0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468,
0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517);
expected = (Mat(5, 5) <<
expected = (Matrix(5, 5) <<
3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000,
@ -1020,13 +1020,13 @@ TEST( matrix, inverse_square_root )
// we are checking against was generated via chol(M)' on octave
TEST( matrix, LLt )
{
Matrix M = (Mat(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464);
Matrix expected = (Mat(5, 5) <<
Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
@ -1039,8 +1039,8 @@ TEST( matrix, LLt )
/* ************************************************************************* */
TEST( matrix, multiplyAdd )
{
Matrix A = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.),
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.),
expected = e + A * x;
multiplyAdd(1, A, x, e);
@ -1050,8 +1050,8 @@ TEST( matrix, multiplyAdd )
/* ************************************************************************* */
TEST( matrix, transposeMultiplyAdd )
{
Matrix A = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.),
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.),
expected = x + trans(A) * e;
transposeMultiplyAdd(1, A, e, x);
@ -1061,31 +1061,31 @@ TEST( matrix, transposeMultiplyAdd )
/* ************************************************************************* */
TEST( matrix, linear_dependent )
{
Matrix A = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Mat(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0);
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0);
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
TEST( matrix, linear_dependent2 )
{
Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0);
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0);
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
TEST( matrix, linear_dependent3 )
{
Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0);
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0);
EXPECT(linear_independent(A, B));
}
/* ************************************************************************* */
TEST( matrix, svd1 )
{
Vector v = (Vec(3) << 2., 1., 0.);
Vector v = (Vector(3) << 2., 1., 0.);
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
* Matrix(trans(V1));
Matrix U, V;
@ -1098,7 +1098,7 @@ TEST( matrix, svd1 )
/* ************************************************************************* */
/// Sample A matrix for SVD
static Matrix sampleA = (Mat(3, 2) << 0.,-2., 0., 0., 3., 0.);
static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.);
static Matrix sampleAt = trans(sampleA);
/* ************************************************************************* */
@ -1107,9 +1107,9 @@ TEST( matrix, svd2 )
Matrix U, V;
Vector s;
Matrix expectedU = (Mat(3, 2) << 0.,-1.,0.,0.,1.,0.);
Vector expected_s = (Vec(2) << 3.,2.);
Matrix expectedV = (Mat(2, 2) << 1.,0.,0.,1.);
Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.);
Vector expected_s = (Vector(2) << 3.,2.);
Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.);
svd(sampleA, U, s, V);
@ -1124,9 +1124,9 @@ TEST( matrix, svd3 )
Matrix U, V;
Vector s;
Matrix expectedU = (Mat(2, 2) << -1.,0.,0.,-1.);
Vector expected_s = (Vec(2) << 3.0, 2.0);
Matrix expectedV = (Mat(3, 2) << 0.,1.,0.,0.,-1.,0.);
Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.);
Vector expected_s = (Vector(2) << 3.0, 2.0);
Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
svd(sampleAt, U, s, V);
Matrix S = diag(s);
@ -1145,19 +1145,19 @@ TEST( matrix, svd4 )
Matrix U, V;
Vector s;
Matrix A = (Mat(3, 2) <<
Matrix A = (Matrix(3, 2) <<
0.8147, 0.9134,
0.9058, 0.6324,
0.1270, 0.0975);
Matrix expectedU = (Mat(3, 2) <<
Matrix expectedU = (Matrix(3, 2) <<
0.7397, 0.6724,
0.6659, -0.7370,
0.0970, -0.0689);
Vector expected_s = (Vec(2) << 1.6455, 0.1910);
Vector expected_s = (Vector(2) << 1.6455, 0.1910);
Matrix expectedV = (Mat(2, 2) <<
Matrix expectedV = (Matrix(2, 2) <<
0.7403, -0.6723,
0.6723, 0.7403);
@ -1173,7 +1173,7 @@ TEST( matrix, svd4 )
/* ************************************************************************* */
TEST( matrix, DLT )
{
Matrix A = (Mat(8, 9) <<
Matrix A = (Matrix(8, 9) <<
0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04,
0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39,
@ -1187,7 +1187,7 @@ TEST( matrix, DLT )
double error;
Vector actual;
boost::tie(rank,error,actual) = DLT(A);
Vector expected = (Vec(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0);
Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0);
EXPECT_LONGS_EQUAL(8,rank);
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
EXPECT(assert_equal(expected, actual, 1e-4));

22
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -31,7 +31,7 @@ double f(const LieVector& x) {
TEST(testNumericalDerivative, numericalHessian) {
LieVector center = ones(2);
Matrix expected = (Mat(2,2) <<
Matrix expected = (Matrix(2,2) <<
-sin(center(0)), 0.0,
0.0, -cos(center(1)));
@ -50,7 +50,7 @@ double f2(const LieVector& x) {
TEST(testNumericalDerivative, numericalHessian2) {
LieVector center(2, 0.5, 1.0);
Matrix expected = (Mat(2,2) <<
Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
@ -69,15 +69,15 @@ double f3(const LieVector& x1, const LieVector& x2) {
TEST(testNumericalDerivative, numericalHessian211) {
LieVector center1(1, 1.0), center2(1, 5.0);
Matrix expected11 = (Mat(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Mat(1, 1) <<-cos(center1(0))*sin(center2(0)));
Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0)));
Matrix actual12 = numericalHessian212(f3, center1, center2);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Mat(1, 1) <<-sin(center1(0))*cos(center2(0)));
Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0)));
Matrix actual22 = numericalHessian222(f3, center1, center2);
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
@ -92,27 +92,27 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
TEST(testNumericalDerivative, numericalHessian311) {
LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Mat(1, 1) << -cos(x)*sin(y)*z*z);
Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = (Mat(1, 1) << cos(x)*cos(y)*2*z);
Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3);
EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3);
EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = (Mat(1, 1) << -sin(x)*sin(y)*2*z);
Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3);
EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = (Mat(1, 1) << sin(x)*cos(y)*2);
Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3);
EXPECT(assert_equal(expected33, actual33, 1e-5));
}

18
gtsam/base/tests/testSerializationBase.cpp
View File

@ -30,9 +30,9 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
Vector v1 = (Vec(2) << 1.0, 2.0);
Vector v2 = (Vec(2) << 3.0, 4.0);
Vector v3 = (Vec(2) << 5.0, 6.0);
Vector v1 = (Vector(2) << 1.0, 2.0);
Vector v2 = (Vector(2) << 3.0, 4.0);
Vector v3 = (Vector(2) << 5.0, 6.0);
/* ************************************************************************* */
TEST (Serialization, FastList) {
@ -84,23 +84,23 @@ TEST (Serialization, FastVector) {
/* ************************************************************************* */
TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equality<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equality<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equality<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equalityXML<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equalityXML<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equalityXML<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equalityBinary<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equalityBinary<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equalityBinary<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
}
/* ************************************************************************* */

22
gtsam/base/tests/testSymmetricBlockMatrix.cpp
View File

@ -25,7 +25,7 @@ using boost::assign::list_of;
static SymmetricBlockMatrix testBlockMatrix(
list_of(3)(2)(1),
(Mat(6, 6) <<
(Matrix(6, 6) <<
1, 2, 3, 4, 5, 6,
2, 8, 9, 10, 11, 12,
3, 9, 15, 16, 17, 18,
@ -37,7 +37,7 @@ static SymmetricBlockMatrix testBlockMatrix(
TEST(SymmetricBlockMatrix, ReadBlocks)
{
// On the diagonal
Matrix expected1 = (Mat(2, 2) <<
Matrix expected1 = (Matrix(2, 2) <<
22, 23,
23, 29);
Matrix actual1 = testBlockMatrix(1, 1);
@ -48,7 +48,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));
// Above the diagonal
Matrix expected2 = (Mat(3, 2) <<
Matrix expected2 = (Matrix(3, 2) <<
4, 5,
10, 11,
16, 17);
@ -56,7 +56,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
EXPECT(assert_equal(expected2, actual2));
// Below the diagonal
Matrix expected3 = (Mat(2, 3) <<
Matrix expected3 = (Matrix(2, 3) <<
4, 10, 16,
5, 11, 17);
Matrix actual3 = testBlockMatrix(1, 0);
@ -99,7 +99,7 @@ TEST(SymmetricBlockMatrix, WriteBlocks)
TEST(SymmetricBlockMatrix, Ranges)
{
// On the diagonal
Matrix expected1 = (Mat(3, 3) <<
Matrix expected1 = (Matrix(3, 3) <<
22, 23, 24,
23, 29, 30,
24, 30, 36);
@ -109,7 +109,7 @@ TEST(SymmetricBlockMatrix, Ranges)
EXPECT(assert_equal(expected1, actual1a));
// Above the diagonal
Matrix expected2 = (Mat(3, 1) <<
Matrix expected2 = (Matrix(3, 1) <<
24,
30,
36);
@ -119,7 +119,7 @@ TEST(SymmetricBlockMatrix, Ranges)
EXPECT(assert_equal(expected2, actual2a));
// Below the diagonal
Matrix expected3 = (Mat(3, 3) <<
Matrix expected3 = (Matrix(3, 3) <<
4, 10, 16,
5, 11, 17,
6, 12, 18);
@ -132,7 +132,7 @@ TEST(SymmetricBlockMatrix, Ranges)
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, expressions)
{
SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Mat(6, 6) <<
SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) <<
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 4, 6, 8, 0,
@ -140,7 +140,7 @@ TEST(SymmetricBlockMatrix, expressions)
0, 0, 0, 0, 16, 0,
0, 0, 0, 0, 0, 0));
SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Mat(6, 6) <<
SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) <<
0, 0, 10, 15, 20, 0,
0, 0, 12, 18, 24, 0,
0, 0, 0, 0, 0, 0,
@ -148,8 +148,8 @@ TEST(SymmetricBlockMatrix, expressions)
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0));
Matrix a = (Mat(1, 3) << 2, 3, 4);
Matrix b = (Mat(1, 2) << 5, 6);
Matrix a = (Matrix(1, 3) << 2, 3, 4);
Matrix b = (Matrix(1, 2) << 5, 6);
SymmetricBlockMatrix bm1(list_of(2)(3)(1));
bm1.full().triangularView().setZero();

66
gtsam/base/tests/testVector.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( TestVector, Vector_variants )
{
Vector a = (Vec(2) << 10.0,20.0);
Vector a = (Vector(2) << 10.0,20.0);
double data[] = {10,20};
Vector b = Vector_(2, data);
EXPECT(assert_equal(a, b));
@ -56,18 +56,18 @@ TEST( TestVector, special_comma_initializer)
expected(1) = 2;
expected(2) = 3;
Vector actual1 = (Vec(3) << 1, 2, 3);
Vector actual2((Vec(3) << 1, 2, 3));
Vector actual3 = (Vec() << 1, 2, 3);
Vector actual1 = (Vector(3) << 1, 2, 3);
Vector actual2((Vector(3) << 1, 2, 3));
Vector actual3 = (Vector() << 1, 2, 3);
Vector subvec1 = (Vec() << 2, 3);
Vector actual4 = (Vec() << 1, subvec1);
Vector subvec1 = (Vector() << 2, 3);
Vector actual4 = (Vector() << 1, subvec1);
Vector subvec2 = (Vec() << 1, 2);
Vector actual5 = (Vec() << subvec2, 3);
Vector subvec2 = (Vector() << 1, 2);
Vector actual5 = (Vector() << subvec2, 3);
Vector actual6 = testFcn1((Vec() << 1, 2, 3));
Vector actual7 = testFcn2((Vec() << 1, 2, 3));
Vector actual6 = testFcn1((Vector() << 1, 2, 3));
Vector actual7 = testFcn2((Vector() << 1, 2, 3));
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
@ -153,7 +153,7 @@ TEST( TestVector, subInsert )
size_t i = 2;
subInsert(big, small, i);
Vector expected = (Vec(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0);
Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0);
EXPECT(assert_equal(expected, big));
}
@ -251,13 +251,13 @@ TEST( TestVector, weightedPseudoinverse_constraint )
/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_nan )
{
Vector a = (Vec(4) << 1., 0., 0., 0.);
Vector sigmas = (Vec(4) << 0.1, 0.1, 0., 0.);
Vector a = (Vector(4) << 1., 0., 0., 0.);
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.);
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
Vector expected = (Vec(4) << 1., 0., 0.,0.);
Vector expected = (Vector(4) << 1., 0., 0.,0.);
EXPECT(assert_equal(expected, pseudo));
DOUBLES_EQUAL(100, precision, 1e-5);
}
@ -265,31 +265,31 @@ TEST( TestVector, weightedPseudoinverse_nan )
/* ************************************************************************* */
TEST( TestVector, ediv )
{
Vector a = (Vec(3) << 10., 20., 30.);
Vector b = (Vec(3) << 2.0, 5.0, 6.0);
Vector a = (Vector(3) << 10., 20., 30.);
Vector b = (Vector(3) << 2.0, 5.0, 6.0);
Vector actual(ediv(a,b));
Vector c = (Vec(3) << 5.0, 4.0, 5.0);
Vector c = (Vector(3) << 5.0, 4.0, 5.0);
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
TEST( TestVector, dot )
{
Vector a = (Vec(3) << 10., 20., 30.);
Vector b = (Vec(3) << 2.0, 5.0, 6.0);
Vector a = (Vector(3) << 10., 20., 30.);
Vector b = (Vector(3) << 2.0, 5.0, 6.0);
DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9);
}
/* ************************************************************************* */
TEST( TestVector, axpy )
{
Vector x = (Vec(3) << 10., 20., 30.);
Vector y0 = (Vec(3) << 2.0, 5.0, 6.0);
Vector x = (Vector(3) << 10., 20., 30.);
Vector y0 = (Vector(3) << 2.0, 5.0, 6.0);
Vector y1 = y0, y2 = y0;
axpy(0.1,x,y1);
axpy(0.1,x,y2.head(3));
Vector expected = (Vec(3) << 3.0, 7.0, 9.0);
Vector expected = (Vector(3) << 3.0, 7.0, 9.0);
EXPECT(assert_equal(expected,y1));
EXPECT(assert_equal(expected,Vector(y2)));
}
@ -297,8 +297,8 @@ TEST( TestVector, axpy )
/* ************************************************************************* */
TEST( TestVector, equals )
{
Vector v1 = (Vec(1) << 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
Vector v2 = (Vec(1) << 1.0);
Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
Vector v2 = (Vector(1) << 1.0);
double tol = 1.;
EXPECT(!equal_with_abs_tol(v1, v2, tol));
}
@ -306,7 +306,7 @@ TEST( TestVector, equals )
/* ************************************************************************* */
TEST( TestVector, greater_than )
{
Vector v1 = (Vec(3) << 1.0, 2.0, 3.0),
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0),
v2 = zero(3);
EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
@ -315,31 +315,31 @@ TEST( TestVector, greater_than )
/* ************************************************************************* */
TEST( TestVector, reciprocal )
{
Vector v = (Vec(3) << 1.0, 2.0, 4.0);
EXPECT(assert_equal((Vec(3) << 1.0, 0.5, 0.25),reciprocal(v)));
Vector v = (Vector(3) << 1.0, 2.0, 4.0);
EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent )
{
Vector v1 = (Vec(3) << 1.0, 2.0, 3.0);
Vector v2 = (Vec(3) << -2.0, -4.0, -6.0);
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0);
Vector v2 = (Vector(3) << -2.0, -4.0, -6.0);
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent2 )
{
Vector v1 = (Vec(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vec(3) << 0.0, -4.0, 0.0);
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vector(3) << 0.0, -4.0, 0.0);
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent3 )
{
Vector v1 = (Vec(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vec(3) << 0.1, -4.1, 0.0);
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vector(3) << 0.1, -4.1, 0.0);
EXPECT(!linear_dependent(v1, v2));
}

6
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -53,10 +53,10 @@ TEST_UNSAFE( DiscreteMarginals, UGM_small ) {
EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6);
Vector actualCvector = marginals.marginalProbabilities(Cathy);
EXPECT(assert_equal((Vec(2) << 0.359631, 0.640369), actualCvector, 1e-6));
EXPECT(assert_equal((Vector(2) << 0.359631, 0.640369), actualCvector, 1e-6));
actualCvector = marginals.marginalProbabilities(Mark);
EXPECT(assert_equal((Vec(2) << 0.48628, 0.51372), actualCvector, 1e-6));
EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372), actualCvector, 1e-6));
}
/* ************************************************************************* */
@ -186,7 +186,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) {
// // solver
// Vector actualV = solver.marginalProbabilities(key[j]);
// EXPECT(assert_equal((Vec(2) << F[j], T[j]), actualV));
// EXPECT(assert_equal((Vector(2) << F[j], T[j]), actualV));
// Marginals
vector<double> table;

6
gtsam/geometry/Cal3Bundler.cpp
View File

@ -42,17 +42,17 @@ Matrix Cal3Bundler::K() const {
/* ************************************************************************* */
Vector Cal3Bundler::k() const {
return (Vec(4) << k1_, k2_, 0, 0);
return (Vector(4) << k1_, k2_, 0, 0);
}
/* ************************************************************************* */
Vector Cal3Bundler::vector() const {
return (Vec(3) << f_, k1_, k2_);
return (Vector(3) << f_, k1_, k2_);
}
/* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const {
gtsam::print((Vector)(Vec(5) << f_, k1_, k2_, u0_, v0_), s + ".K");
gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_), s + ".K");
}
/* ************************************************************************* */

16
gtsam/geometry/Cal3DS2.cpp
View File

@ -28,10 +28,10 @@ Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const { return (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
/* ************************************************************************* */
Vector Cal3DS2::vector() const { return (Vec(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
Vector Cal3DS2::vector() const { return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); }
@ -70,7 +70,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
// Inlined derivative for calibration
if (H1) {
*H1 = (Mat(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
*H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
@ -87,8 +87,8 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
Matrix DK = (Mat(2, 2) << fx, s_, 0.0, fy);
Matrix DR = (Mat(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
*H2 = DK * DR;
@ -148,8 +148,8 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double dDy_dx = 2*k4*y + k3*dr_dx;
const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
Matrix DK = (Mat(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Mat(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
return DK * DR;
@ -167,7 +167,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double pnx = g*x + dx;
const double pny = g*y + dy;
return (Mat(2, 9) <<
return (Matrix(2, 9) <<
pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
}

4
gtsam/geometry/Cal3_S2.cpp
View File

@ -76,9 +76,9 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal = (Mat(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
*Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
if (Dp)
*Dp = (Mat(2, 2) << fx_, s_, 0.000, fy_);
*Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_);
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}

4
gtsam/geometry/Cal3_S2.h
View File

@ -126,7 +126,7 @@ public:
/// return calibration matrix K
Matrix K() const {
return (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/** @deprecated The following function has been deprecated, use K above */
@ -137,7 +137,7 @@ public:
/// return inverted calibration matrix inv(K)
Matrix matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
return (Mat(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
}

6
gtsam/geometry/CalibratedCamera.cpp
View File

@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = (Mat(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
*H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@ -84,12 +84,12 @@ Point2 CalibratedCamera::project(const Point3& point,
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose = (Mat(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
*Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v);
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
*Dpoint = d
* (Mat(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
* (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2));
}

2
gtsam/geometry/PinholeCamera.h
View File

@ -273,7 +273,7 @@ public:
boost::optional<Matrix&> Dpoint = boost::none) {
if (Dpoint) {
double d = 1.0 / P.z(), d2 = d * d;
*Dpoint = (Mat(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
*Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}

4
gtsam/geometry/Point2.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2);
static const Matrix oneOne = (Mat(1, 2) << 1.0, 1.0);
static const Matrix oneOne = (Matrix(1, 2) << 1.0, 1.0);
/* ************************************************************************* */
void Point2::print(const string& s) const {
@ -45,7 +45,7 @@ double Point2::norm(boost::optional<Matrix&> H) const {
if (H) {
Matrix D_result_d;
if (fabs(r) > 1e-10)
D_result_d = (Mat(1, 2) << x_ / r, y_ / r);
D_result_d = (Matrix(1, 2) << x_ / r, y_ / r);
else
D_result_d = oneOne; // TODO: really infinity, why 1 here??
*H = D_result_d;

22
gtsam/geometry/Pose2.cpp
View File

@ -40,7 +40,7 @@ static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = (Mat(3, 1) << t_.x(), t_.y(), 1.0);
Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
@ -75,13 +75,13 @@ Vector Pose2::Logmap(const Pose2& p) {
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return (Vec(3) << t.x(), t.y(), w);
return (Vector(3) << t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vec(3) << v.x(), v.y(), w);
return (Vector(3) << v.x(), v.y(), w);
}
}
@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
return Logmap(between(p2));
#else
Pose2 r = between(p2);
return (Vec(3) << r.x(), r.y(), r.theta());
return (Vector(3) << r.x(), r.y(), r.theta());
#endif
}
@ -110,7 +110,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return (Mat(3, 3) <<
return (Matrix(3, 3) <<
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
@ -130,7 +130,7 @@ Point2 Pose2::transform_to(const Point2& point,
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = (Mat(2, 3) <<
if (H1) *H1 = (Matrix(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
@ -154,7 +154,7 @@ Point2 Pose2::transform_from(const Point2& p,
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = (Mat(2, 1) << -q.y(), q.x());
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
@ -184,7 +184,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = (Mat(3, 3) <<
*H1 = (Matrix(3, 3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0);
@ -225,7 +225,7 @@ double Pose2::range(const Point2& point,
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * (Mat(2, 3) <<
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H;
@ -240,10 +240,10 @@ double Pose2::range(const Pose2& pose2,
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * (Mat(2, 3) <<
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H * (Mat(2, 3) <<
if (H2) *H2 = H * (Matrix(2, 3) <<
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0);
return r;

2
gtsam/geometry/Pose2.h
View File

@ -172,7 +172,7 @@ public:
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return (Mat(3,3) <<
return (Matrix(3,3) <<
0.,-w, vx,
w, 0., vy,
0., 0., 0.);

2
gtsam/geometry/Pose3.cpp
View File

@ -306,7 +306,7 @@ double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2);
Matrix D_result_d = (Mat(1, 3) << x / n, y / n, z / n);
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n);
if (H1)
*H1 = D_result_d * (*H1);
if (H2)

2
gtsam/geometry/Pose3.h
View File

@ -218,7 +218,7 @@ public:
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return (Mat(4,4) <<
return (Matrix(4,4) <<
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,

12
gtsam/geometry/Rot2.cpp
View File

@ -65,12 +65,12 @@ Rot2& Rot2::normalize() {
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return (Mat(2, 2) << c_, -s_, s_, c_);
return (Matrix(2, 2) << c_, -s_, s_, c_);
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return (Mat(2, 2) << c_, s_, -s_, c_);
return (Matrix(2, 2) << c_, s_, -s_, c_);
}
/* ************************************************************************* */
@ -78,7 +78,7 @@ Matrix Rot2::transpose() const {
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Mat(2, 1) << -q.y(), q.x());
if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x());
if (H2) *H2 = matrix();
return q;
}
@ -88,7 +88,7 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Mat(2, 1) << q.y(), -q.x()); // R_{pi/2}q
if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()); // R_{pi/2}q
if (H2) *H2 = transpose();
return q;
}
@ -97,10 +97,10 @@ Point2 Rot2::unrotate(const Point2& p,
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) *H = (Mat(1, 2) << -y / d2, x / d2);
if (H) *H = (Matrix(1, 2) << -y / d2, x / d2);
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = (Mat(1, 2) << 0.0, 0.0);
if (H) *H = (Matrix(1, 2) << 0.0, 0.0);
return Rot2();
}
}

6
gtsam/geometry/StereoCamera.cpp
View File

@ -64,7 +64,7 @@ namespace gtsam {
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
*H1 = (Mat(3, 6) <<
*H1 = (Matrix(3, 6) <<
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v
@ -72,7 +72,7 @@ namespace gtsam {
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
*H2 = d * (Mat(3, 3) <<
*H2 = d * (Matrix(3, 3) <<
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
@ -90,7 +90,7 @@ namespace gtsam {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
return (Mat(3, 3) <<
return (Matrix(3, 3) <<
f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y()

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -27,7 +27,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
static const Pose3 pose1((Matrix)(Mat(3,3) <<
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -29,7 +29,7 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix)(Mat(3,3) <<
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.

6
gtsam/geometry/tests/testPoint2.cpp
View File

@ -46,8 +46,8 @@ TEST(Point2, Lie) {
EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(5,7), p1.retract((Vec(2) << 4., 5.))));
EXPECT(assert_equal((Vec(2) << 3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.))));
EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */
@ -101,7 +101,7 @@ TEST( Point2, norm ) {
// exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
actual = x1.norm(actualH);
EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
expectedH = (Mat(1, 2) << 1.0, 1.0);
expectedH = (Matrix(1, 2) << 1.0, 1.0);
EXPECT(assert_equal(expectedH,actualH));
actual = x2.norm(actualH);

2
gtsam/geometry/tests/testPoint3.cpp
View File

@ -39,7 +39,7 @@ TEST(Point3, Lie) {
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vec(3) << 4., 5., 6.))));
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
}

52
gtsam/geometry/tests/testPose2.cpp
View File

@ -68,7 +68,7 @@ TEST(Pose2, retract) {
#else
Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
#endif
Pose2 actual = pose.retract((Vec(3) << 0.01, -0.015, 0.99));
Pose2 actual = pose.retract((Vector(3) << 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -76,7 +76,7 @@ TEST(Pose2, retract) {
TEST(Pose2, expmap) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, (Vec(3) << 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -84,7 +84,7 @@ TEST(Pose2, expmap) {
TEST(Pose2, expmap2) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, (Vec(3) << 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -92,14 +92,14 @@ TEST(Pose2, expmap2) {
TEST(Pose2, expmap3) {
// do an actual series exponential map
// see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
Matrix A = (Mat(3,3) <<
Matrix A = (Matrix(3,3) <<
0.0, -0.99, 0.01,
0.99, 0.0, -0.015,
0.0, 0.0, 0.0);
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Vector v = (Vec(3) << 0.01, -0.015, 0.99);
Vector v = (Vector(3) << 0.01, -0.015, 0.99);
Pose2 pose = Pose2::Expmap(v);
Pose2 pose2(v);
EXPECT(assert_equal(pose, pose2));
@ -110,7 +110,7 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */
TEST(Pose2, expmap0a) {
Pose2 expected(0.0101345, -0.0149092, 0.018);
Pose2 actual = Pose2::Expmap((Vec(3) << 0.01, -0.015, 0.018));
Pose2 actual = Pose2::Expmap((Vector(3) << 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -118,7 +118,7 @@ TEST(Pose2, expmap0a) {
TEST(Pose2, expmap0b) {
// a quarter turn
Pose2 expected(1.0, 1.0, M_PI/2);
Pose2 actual = Pose2::Expmap((Vec(3) << M_PI/2, 0.0, M_PI/2));
Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -126,7 +126,7 @@ TEST(Pose2, expmap0b) {
TEST(Pose2, expmap0c) {
// a half turn
Pose2 expected(0.0, 2.0, M_PI);
Pose2 actual = Pose2::Expmap((Vec(3) << M_PI, 0.0, M_PI));
Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -134,7 +134,7 @@ TEST(Pose2, expmap0c) {
TEST(Pose2, expmap0d) {
// a full turn
Pose2 expected(0, 0, 0);
Pose2 actual = Pose2::Expmap((Vec(3) << 2*M_PI, 0.0, 2*M_PI));
Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) {
// test case for screw motion in the plane
namespace screw {
double w=0.3;
Vector xi = (Vec(3) << 0.0, w, w);
Vector xi = (Vector(3) << 0.0, w, w);
Rot2 expectedR = Rot2::fromAngle(w);
Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT);
@ -161,7 +161,7 @@ TEST(Pose3, expmap_c)
TEST(Pose2, expmap_c_full)
{
double w=0.3;
Vector xi = (Vec(3) << 0.0, w, w);
Vector xi = (Vector(3) << 0.0, w, w);
Rot2 expectedR = Rot2::fromAngle(w);
Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT);
@ -175,9 +175,9 @@ TEST(Pose2, logmap) {
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP
Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018);
Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018);
#else
Vector expected = (Vec(3) << 0.01, -0.015, 0.018);
Vector expected = (Vector(3) << 0.01, -0.015, 0.018);
#endif
Vector actual = pose0.localCoordinates(pose);
EXPECT(assert_equal(expected, actual, 1e-5));
@ -187,7 +187,7 @@ TEST(Pose2, logmap) {
TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018);
Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018);
Vector actual = logmap_default<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -204,8 +204,8 @@ TEST( Pose2, transform_to )
// expected
Point2 expected(2,2);
Matrix expectedH1 = (Mat(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0);
Matrix expectedH2 = (Mat(2,2) << 0.0, 1.0, -1.0, 0.0);
Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0);
Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0);
// actual
Matrix actualH1, actualH2;
@ -236,8 +236,8 @@ TEST (Pose2, transform_from)
Point2 expected(0., 2.);
EXPECT(assert_equal(expected, actual));
Matrix H1_expected = (Mat(2, 3) << 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = (Mat(2, 2) << 0., -1., 1., 0.);
Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt);
EXPECT(assert_equal(H1_expected, H1));
@ -261,7 +261,7 @@ TEST(Pose2, compose_a)
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
EXPECT(assert_equal(expected, actual));
Matrix expectedH1 = (Mat(3,3) <<
Matrix expectedH1 = (Matrix(3,3) <<
0.0, 1.0, 0.0,
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
@ -348,14 +348,14 @@ TEST(Pose2, inverse )
namespace {
/* ************************************************************************* */
Vector homogeneous(const Point2& p) {
return (Vec(3) << p.x(), p.y(), 1.0);
return (Vector(3) << p.x(), p.y(), 1.0);
}
/* ************************************************************************* */
Matrix matrix(const Pose2& gTl) {
Matrix gRl = gTl.r().matrix();
Point2 gt = gTl.t();
return (Mat(3, 3) <<
return (Matrix(3, 3) <<
gRl(0, 0), gRl(0, 1), gt.x(),
gRl(1, 0), gRl(1, 1), gt.y(),
0.0, 0.0, 1.0);
@ -368,7 +368,7 @@ TEST( Pose2, matrix )
Point2 origin, t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
EXPECT(assert_equal((Mat(3,3) <<
EXPECT(assert_equal((Matrix(3,3) <<
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
@ -376,7 +376,7 @@ TEST( Pose2, matrix )
Rot2 gR1 = gTl.r();
EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
EXPECT(assert_equal((Mat(2,2) <<
EXPECT(assert_equal((Matrix(2,2) <<
0.0, -1.0,
1.0, 0.0),
gR1.matrix()));
@ -387,7 +387,7 @@ TEST( Pose2, matrix )
// check inverse pose
Matrix lMg = matrix(gTl.inverse());
EXPECT(assert_equal((Mat(3,3) <<
EXPECT(assert_equal((Matrix(3,3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
@ -421,7 +421,7 @@ TEST( Pose2, between )
EXPECT(assert_equal(expected,actual1));
EXPECT(assert_equal(expected,actual2));
Matrix expectedH1 = (Mat(3,3) <<
Matrix expectedH1 = (Matrix(3,3) <<
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
@ -432,7 +432,7 @@ TEST( Pose2, between )
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
Matrix expectedH2 = (Mat(3,3) <<
Matrix expectedH2 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0

20
gtsam/geometry/tests/testPose3.cpp
View File

@ -104,7 +104,7 @@ TEST( Pose3, expmap_a_full2)
TEST(Pose3, expmap_b)
{
Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = p1.retract((Vec(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2,1e-2));
}
@ -113,7 +113,7 @@ TEST(Pose3, expmap_b)
// test case for screw motion in the plane
namespace screw {
double a=0.3, c=cos(a), s=sin(a), w=0.3;
Vector xi = (Vec(6) << 0.0, 0.0, w, w, 0.0, 1.0);
Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0);
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
Point3 expectedT(0.29552, 0.0446635, 1);
Pose3 expected(expectedR, expectedT);
@ -163,13 +163,13 @@ Pose3 Agrawal06iros(const Vector& xi) {
TEST(Pose3, expmaps_galore_full)
{
Vector xi; Pose3 actual;
xi = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
xi = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
xi = (Vec(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6);
xi = (Vector(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta;
actual = Pose3::Expmap(txi);
@ -181,7 +181,7 @@ TEST(Pose3, expmaps_galore_full)
}
// Works with large v as well, but expm needs 10 iterations!
xi = (Vec(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0);
xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0);
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
@ -194,7 +194,7 @@ TEST(Pose3, Adjoint_compose_full)
// To debug derivatives of compose, assert that
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T;
Vector x = (Vec(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8);
Vector x = (Vector(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8);
Pose3 expected = T1 * Pose3::Expmap(x) * T2;
Vector y = T2.inverse().Adjoint(x);
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
@ -510,7 +510,7 @@ TEST(Pose3, subgroups)
{
// Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
Vector d = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
// exp(-d)=inverse(exp(d))
EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
@ -675,7 +675,7 @@ TEST(Pose3, align_2) {
/// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*exp(xi+x))
Vector xi = (Vec(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
Vector testDerivExpmapInv(const LieVector& dxi) {
Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi));
@ -723,8 +723,8 @@ Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) {
}
TEST( Pose3, adjointTranspose) {
Vector xi = (Vec(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0);
Vector v = (Vec(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0);
Vector xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0);
Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0);
Vector expected = testDerivAdjointTranspose(xi, v);
Matrix actualH;

50
gtsam/geometry/tests/testRot3M.cpp
View File

@ -57,7 +57,7 @@ TEST( Rot3, constructor)
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
@ -101,7 +101,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vec(3) << epsilon, 0., 0.);
Vector w = (Vector(3) << epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
@ -109,7 +109,7 @@ TEST( Rot3, rodriguez)
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vec(3) << 0., 1., 0.); // rotation around Y
Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
@ -121,7 +121,7 @@ TEST( Rot3, rodriguez2)
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vec(3) << 0.1, 0.2, 0.3);
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
@ -130,7 +130,7 @@ TEST( Rot3, rodriguez3)
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
@ -156,7 +156,7 @@ TEST(Rot3, log)
Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \
w = (Vec(3) << (double)X, (double)Y, double(Z)); \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
@ -190,7 +190,7 @@ TEST(Rot3, log)
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vec(3) << (double)X, (double)Y, double(Z)); \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
@ -217,7 +217,7 @@ TEST(Rot3, manifold_caley)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vec(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
@ -245,7 +245,7 @@ TEST(Rot3, manifold_slow_caley)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vec(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
@ -277,7 +277,7 @@ TEST(Rot3, manifold_expmap)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vec(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
@ -403,7 +403,7 @@ TEST( Rot3, between )
}
/* ************************************************************************* */
Vector w = (Vec(3) << 0.1, 0.27, -0.2);
Vector w = (Vector(3) << 0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
@ -473,7 +473,7 @@ TEST( Rot3, yaw_pitch_roll )
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
CHECK(assert_equal((Vector)(Vec(3) << 0.1, 0.2, 0.3),expected.ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr()));
}
/* ************************************************************************* */
@ -483,25 +483,25 @@ TEST( Rot3, RQ)
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671);
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(I3,actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vec(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
@ -510,11 +510,11 @@ TEST( Rot3, RQ)
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7);
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
@ -526,7 +526,7 @@ TEST( Rot3, expmapStability ) {
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
Vector w = (Vec(3) << 1e-8, 0.0, 0.0);
Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
@ -541,13 +541,13 @@ TEST( Rot3, logmapStability ) {
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Mat(3, 3) <<
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Matrix)(Mat(3, 3) <<
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));

56
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -52,7 +52,7 @@ TEST( Rot3, constructor)
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
@ -88,7 +88,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vec(3) << epsilon, 0., 0.);
Vector w = (Vector(3) << epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
@ -96,7 +96,7 @@ TEST( Rot3, rodriguez)
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vec(3) << 0.,1.,0.); // rotation around Y
Vector axis = (Vector(3) << 0.,1.,0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
@ -108,7 +108,7 @@ TEST( Rot3, rodriguez2)
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vec(3) << 0.1, 0.2, 0.3);
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
@ -117,7 +117,7 @@ TEST( Rot3, rodriguez3)
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
double angle = M_PI_2;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
@ -138,35 +138,35 @@ TEST( Rot3, expmap)
/* ************************************************************************* */
TEST(Rot3, log)
{
Vector w1 = (Vec(3) << 0.1, 0.0, 0.0);
Vector w1 = (Vector(3) << 0.1, 0.0, 0.0);
Rot3 R1 = Rot3::rodriguez(w1);
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
Vector w2 = (Vec(3) << 0.0, 0.1, 0.0);
Vector w2 = (Vector(3) << 0.0, 0.1, 0.0);
Rot3 R2 = Rot3::rodriguez(w2);
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
Vector w3 = (Vec(3) << 0.0, 0.0, 0.1);
Vector w3 = (Vector(3) << 0.0, 0.0, 0.1);
Rot3 R3 = Rot3::rodriguez(w3);
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
Vector w = (Vec(3) << 0.1, 0.4, 0.2);
Vector w = (Vector(3) << 0.1, 0.4, 0.2);
Rot3 R = Rot3::rodriguez(w);
CHECK(assert_equal(w, Rot3::Logmap(R)));
Vector w5 = (Vec(3) << 0.0, 0.0, 0.0);
Vector w5 = (Vector(3) << 0.0, 0.0, 0.0);
Rot3 R5 = Rot3::rodriguez(w5);
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
Vector w6 = (Vec(3) << boost::math::constants::pi<double>(), 0.0, 0.0);
Vector w6 = (Vector(3) << boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = Rot3::rodriguez(w6);
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
Vector w7 = (Vec(3) << 0.0, boost::math::constants::pi<double>(), 0.0);
Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = Rot3::rodriguez(w7);
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
Vector w8 = (Vec(3) << 0.0, 0.0, boost::math::constants::pi<double>());
Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = Rot3::rodriguez(w8);
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
}
@ -190,7 +190,7 @@ TEST(Rot3, manifold)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vec(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
@ -298,7 +298,7 @@ TEST( Rot3, between )
Rot3 r1 = Rot3::Rz(M_PI/3.0);
Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
Matrix expectedr1 = (Mat(3, 3) <<
Matrix expectedr1 = (Matrix(3, 3) <<
0.5, -sqrt(3.0)/2.0, 0.0,
sqrt(3.0)/2.0, 0.5, 0.0,
0.0, 0.0, 1.0);
@ -381,25 +381,25 @@ TEST( Rot3, RQ)
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671);
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vec(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
@ -408,11 +408,11 @@ TEST( Rot3, RQ)
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7);
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
@ -425,7 +425,7 @@ TEST( Rot3, expmapStability ) {
// Does not work with Quaternions
///* ************************************************************************* */
//TEST( Rot3, logmapStability ) {
// Vector w = (Vec(3) << 1e-8, 0.0, 0.0);
// Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
// Rot3 R = Rot3::Expmap(w);
//// double tr = R.r1().x()+R.r2().y()+R.r3().z();
//// std::cout.precision(5000);
@ -440,13 +440,13 @@ TEST( Rot3, expmapStability ) {
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Mat(3, 3) <<
Rot3 R1 = Rot3((Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Mat(3, 3) <<
Rot3 R2 = Rot3((Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));

4
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -28,7 +28,7 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix)(Mat(3,3) <<
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -144,7 +144,7 @@ TEST( SimpleCamera, simpleCamera)
Point3 T(1000,2000,1500);
SimpleCamera expected(Pose3(R.inverse(),T),K);
// H&Z example, 2nd edition, page 163
Matrix P = (Mat(3,4) <<
Matrix P = (Matrix(3,4) <<
3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
-1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);

2
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -74,7 +74,7 @@ TEST( StereoCamera, project)
/* ************************************************************************* */
static Pose3 camera1((Matrix)(Mat(3,3) <<
static Pose3 camera1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.

4
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -44,8 +44,8 @@ TEST(StereoPoint2, Lie) {
EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2)));
EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vec(3) << 4., 5., 6.))));
EXPECT(assert_equal((Vec(3) << 3., 3., 3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal((Vector(3) << 3., 3., 3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */

4
gtsam/linear/tests/testErrors.cpp
View File

@ -29,12 +29,12 @@ using namespace gtsam;
TEST( Errors, arithmetic )
{
Errors e;
e += (Vec(2) << 1.0,2.0), (Vec(3) << 3.0,4.0,5.0);
e += (Vector(2) << 1.0,2.0), (Vector(3) << 3.0,4.0,5.0);
DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9);
axpy(2.0,e,e);
Errors expected;
expected += (Vec(2) << 3.0,6.0), (Vec(3) << 9.0,12.0,15.0);
expected += (Vector(2) << 3.0,6.0), (Vector(3) << 9.0,12.0,15.0);
CHECK(assert_equal(expected,e));
}

60
gtsam/linear/tests/testGaussianBayesNetUnordered.cpp
View File

@ -38,8 +38,8 @@ using namespace gtsam;
static const Key _x_=0, _y_=1, _z_=2;
static GaussianBayesNet smallBayesNet = list_of
(GaussianConditional(_x_, (Vec(1) << 9.0), (Mat(1, 1) << 1.0), _y_, (Mat(1, 1) << 1.0)))
(GaussianConditional(_y_, (Vec(1) << 5.0), (Mat(1, 1) << 1.0)));
(GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0)))
(GaussianConditional(_y_, (Vector(1) << 5.0), (Matrix(1, 1) << 1.0)));
/* ************************************************************************* */
TEST( GaussianBayesNet, matrix )
@ -47,11 +47,11 @@ TEST( GaussianBayesNet, matrix )
Matrix R; Vector d;
boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS
Matrix R1 = (Mat(2, 2) <<
Matrix R1 = (Matrix(2, 2) <<
1.0, 1.0,
0.0, 1.0
);
Vector d1 = (Vec(2) << 9.0, 5.0);
Vector d1 = (Vector(2) << 9.0, 5.0);
EXPECT(assert_equal(R,R1));
EXPECT(assert_equal(d,d1));
@ -63,8 +63,8 @@ TEST( GaussianBayesNet, optimize )
VectorValues actual = smallBayesNet.optimize();
VectorValues expected = map_list_of<Key, Vector>
(_x_, (Vec(1) << 4.0))
(_y_, (Vec(1) << 5.0));
(_x_, (Vector(1) << 4.0))
(_y_, (Vector(1) << 5.0));
EXPECT(assert_equal(expected,actual));
}
@ -78,13 +78,13 @@ TEST( GaussianBayesNet, optimize3 )
// NOTE: we are supplying a new RHS here
VectorValues expected = map_list_of<Key, Vector>
(_x_, (Vec(1) << -1.0))
(_y_, (Vec(1) << 5.0));
(_x_, (Vector(1) << -1.0))
(_y_, (Vector(1) << 5.0));
// Test different RHS version
VectorValues gx = map_list_of<Key, Vector>
(_x_, (Vec(1) << 4.0))
(_y_, (Vec(1) << 5.0));
(_x_, (Vector(1) << 4.0))
(_y_, (Vector(1) << 5.0));
VectorValues actual = smallBayesNet.backSubstitute(gx);
EXPECT(assert_equal(expected, actual));
}
@ -97,11 +97,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
// 5 1 1 3
VectorValues
x = map_list_of<Key, Vector>
(_x_, (Vec(1) << 2.0))
(_y_, (Vec(1) << 5.0)),
(_x_, (Vector(1) << 2.0))
(_y_, (Vector(1) << 5.0)),
expected = map_list_of<Key, Vector>
(_x_, (Vec(1) << 2.0))
(_y_, (Vec(1) << 3.0));
(_x_, (Vector(1) << 2.0))
(_y_, (Vector(1) << 3.0));
VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
EXPECT(assert_equal(expected, actual));
@ -113,15 +113,15 @@ TEST( GaussianBayesNet, DeterminantTest )
{
GaussianBayesNet cbn;
cbn += GaussianConditional(
0, (Vec(2) << 3.0, 4.0 ), (Mat(2, 2) << 1.0, 3.0, 0.0, 4.0 ),
1, (Mat(2, 2) << 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0));
0, (Vector(2) << 3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ),
1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
1, (Vec(2) << 5.0, 6.0 ), (Mat(2, 2) << 1.0, 1.0, 0.0, 3.0 ),
2, (Mat(2, 2) << 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0));
1, (Vector(2) << 5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ),
2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
3, (Vec(2) << 7.0, 8.0 ), (Mat(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0));
3, (Vector(2) << 7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0));
double expectedDeterminant = 60.0 / 64.0;
double actualDeterminant = cbn.determinant();
@ -144,21 +144,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0),
3, (Mat(2, 2) << 7.0,8.0,9.0,10.0),
4, (Mat(2, 2) << 11.0,12.0,13.0,14.0)));
0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0),
3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0),
4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0),
2, (Mat(2, 2) << 21.0,22.0,23.0,24.0),
4, (Mat(2, 2) << 25.0,26.0,27.0,28.0)));
1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0),
2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0),
4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0),
3, (Mat(2, 2) << 35.0,36.0,37.0,38.0)));
2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0),
3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0),
4, (Mat(2, 2) << 45.0,46.0,47.0,48.0)));
3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0),
4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0)));
4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(

54
gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
View File

@ -38,10 +38,10 @@ namespace {
const Key x1=1, x2=2, x3=3, x4=4;
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, (Mat(1, 1) << 1.), x1, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, (Mat(1, 1) << 1.), x3, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x3, (Mat(1, 1) << 1.), x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise));
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise));
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
/* ************************************************************************* */
@ -84,13 +84,13 @@ TEST( GaussianBayesTree, eliminate )
{
GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering);
Matrix two = (Mat(1, 1) << 2.);
Matrix one = (Mat(1, 1) << 1.);
Matrix two = (Matrix(1, 1) << 2.);
Matrix one = (Matrix(1, 1) << 1.);
GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot(
MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x3, (Mat(2, 1) << 2., 0.)) (x4, (Mat(2, 1) << 2., 2.)), 2, (Vec(2) << 2., 2.)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x2, (Mat(2, 1) << -2.*sqrt(2.), 0.)) (x1, (Mat(2, 1) << -sqrt(2.), -sqrt(2.))) (x3, (Mat(2, 1) << -sqrt(2.), sqrt(2.))), 2, (Vec(2) << -2.*sqrt(2.), 0.))))));
MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x3, (Matrix(2, 1) << 2., 0.)) (x4, (Matrix(2, 1) << 2., 2.)), 2, (Vector(2) << 2., 2.)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.)) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.))) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.))), 2, (Vector(2) << -2.*sqrt(2.), 0.))))));
EXPECT(assert_equal(bayesTree_expected, bt));
}
@ -99,10 +99,10 @@ TEST( GaussianBayesTree, eliminate )
TEST( GaussianBayesTree, optimizeMultiFrontal )
{
VectorValues expected = pair_list_of<Key, Vector>
(x1, (Vec(1) << 0.))
(x2, (Vec(1) << 1.))
(x3, (Vec(1) << 0.))
(x4, (Vec(1) << 1.));
(x1, (Vector(1) << 0.))
(x2, (Vector(1) << 1.))
(x3, (Vector(1) << 0.))
(x4, (Vector(1) << 1.));
VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
EXPECT(assert_equal(expected,actual));
@ -191,56 +191,56 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
GaussianBayesTree bt;
bt.insertRoot(MakeClique(GaussianConditional(
pair_list_of<Key, Matrix>
(2, (Mat(6, 2) <<
(2, (Matrix(6, 2) <<
31.0,32.0,
0.0,34.0,
0.0,0.0,
0.0,0.0,
0.0,0.0,
0.0,0.0))
(3, (Mat(6, 2) <<
(3, (Matrix(6, 2) <<
35.0,36.0,
37.0,38.0,
41.0,42.0,
0.0,44.0,
0.0,0.0,
0.0,0.0))
(4, (Mat(6, 2) <<
(4, (Matrix(6, 2) <<
0.0,0.0,
0.0,0.0,
45.0,46.0,
47.0,48.0,
51.0,52.0,
0.0,54.0)),
3, (Vec(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of
3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of
(MakeClique(GaussianConditional(
pair_list_of<Key, Matrix>
(0, (Mat(4, 2) <<
(0, (Matrix(4, 2) <<
3.0,4.0,
0.0,6.0,
0.0,0.0,
0.0,0.0))
(1, (Mat(4, 2) <<
(1, (Matrix(4, 2) <<
0.0,0.0,
0.0,0.0,
17.0,18.0,
0.0,20.0))
(2, (Mat(4, 2) <<
(2, (Matrix(4, 2) <<
0.0,0.0,
0.0,0.0,
21.0,22.0,
23.0,24.0))
(3, (Mat(4, 2) <<
(3, (Matrix(4, 2) <<
7.0,8.0,
9.0,10.0,
0.0,0.0,
0.0,0.0))
(4, (Mat(4, 2) <<
(4, (Matrix(4, 2) <<
11.0,12.0,
13.0,14.0,
25.0,26.0,
27.0,28.0)),
2, (Vec(4) << 1.0,2.0,15.0,16.0))))));
2, (Vector(4) << 1.0,2.0,15.0,16.0))))));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
@ -264,11 +264,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Known steepest descent point from Bayes' net version
VectorValues expectedFromBN = pair_list_of<Key, Vector>
(0, (Vec(2) << 0.000129034, 0.000688183))
(1, (Vec(2) << 0.0109679, 0.0253767))
(2, (Vec(2) << 0.0680441, 0.114496))
(3, (Vec(2) << 0.16125, 0.241294))
(4, (Vec(2) << 0.300134, 0.423233));
(0, (Vector(2) << 0.000129034, 0.000688183))
(1, (Vector(2) << 0.0109679, 0.0253767))
(2, (Vector(2) << 0.0680441, 0.114496))
(3, (Vector(2) << 0.16125, 0.241294))
(4, (Vector(2) << 0.300134, 0.423233));
// Compute the steepest descent point with the dogleg function
VectorValues actual = bt.optimizeGradientSearch();

44
gtsam/linear/tests/testGaussianConditionalUnordered.cpp
View File

@ -39,25 +39,25 @@ using namespace boost::assign;
static const double tol = 1e-5;
static Matrix R = (Mat(2, 2) <<
static Matrix R = (Matrix(2, 2) <<
-12.1244, -5.1962,
0., 4.6904);
/* ************************************************************************* */
TEST(GaussianConditional, constructor)
{
Matrix S1 = (Mat(2, 2) <<
Matrix S1 = (Matrix(2, 2) <<
-5.2786, -8.6603,
5.0254, 5.5432);
Matrix S2 = (Mat(2, 2) <<
Matrix S2 = (Matrix(2, 2) <<
-10.5573, -5.9385,
5.5737, 3.0153);
Matrix S3 = (Mat(2, 2) <<
Matrix S3 = (Matrix(2, 2) <<
-11.3820, -7.2581,
-3.0153, -3.5635);
Vector d = (Vec(2) << 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vec(2) << 3.0, 4.0));
Vector d = (Vector(2) << 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0));
vector<pair<Key, Matrix> > terms = pair_list_of
(1, R)
@ -114,9 +114,9 @@ TEST( GaussianConditional, equals )
R(0,0) = 0.1 ; R(1,0) = 0.3;
R(0,1) = 0.0 ; R(1,1) = 0.34;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(2) << 1.0, 0.34));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 0.34));
Vector d = (Vec(2) << 0.2, 0.5);
Vector d = (Vector(2) << 0.2, 0.5);
GaussianConditional
expected(1, d, R, 2, A1, 10, A2, model),
@ -133,13 +133,13 @@ TEST( GaussianConditional, solve )
expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
// create a conditional Gaussian node
Matrix R = (Mat(2, 2) << 1., 0.,
Matrix R = (Matrix(2, 2) << 1., 0.,
0., 1.);
Matrix A1 = (Mat(2, 2) << 1., 2.,
Matrix A1 = (Matrix(2, 2) << 1., 2.,
3., 4.);
Matrix A2 = (Mat(2, 2) << 5., 6.,
Matrix A2 = (Matrix(2, 2) << 5., 6.,
7., 8.);
Vector d(2); d << 20.0, 40.0;
@ -177,7 +177,7 @@ TEST( GaussianConditional, solve_simple )
GaussianConditional cg(list_of(1)(2), 1, blockMatrix);
// partial solution
Vector sx1 = (Vec(2) << 9.0, 10.0);
Vector sx1 = (Vector(2) << 9.0, 10.0);
// elimination order: 1, 2
VectorValues actual = map_list_of
@ -185,7 +185,7 @@ TEST( GaussianConditional, solve_simple )
VectorValues expected = map_list_of<Key, Vector>
(2, sx1)
(1, (Vec(4) << -3.1,-3.4,-11.9,-13.2));
(1, (Vector(4) << -3.1,-3.4,-11.9,-13.2));
// verify indices/size
EXPECT_LONGS_EQUAL(2, (long)cg.size());
@ -213,15 +213,15 @@ TEST( GaussianConditional, solve_multifrontal )
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d()));
// partial solution
Vector sl1 = (Vec(2) << 9.0, 10.0);
Vector sl1 = (Vector(2) << 9.0, 10.0);
// elimination order; _x_, _x1_, _l1_
VectorValues actual = map_list_of
(10, sl1); // parent
VectorValues expected = map_list_of<Key, Vector>
(1, (Vector)(Vec(2) << -3.1,-3.4))
(2, (Vector)(Vec(2) << -11.9,-13.2))
(1, (Vector)(Vector(2) << -3.1,-3.4))
(2, (Vector)(Vector(2) << -11.9,-13.2))
(10, sl1);
// verify indices/size
@ -241,8 +241,8 @@ TEST( GaussianConditional, solveTranspose ) {
* 1 1 9
* 1 5
*/
Matrix R11 = (Mat(1, 1) << 1.0), S12 = (Mat(1, 1) << 1.0);
Matrix R22 = (Mat(1, 1) << 1.0);
Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0);
Matrix R22 = (Matrix(1, 1) << 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
@ -258,11 +258,11 @@ TEST( GaussianConditional, solveTranspose ) {
VectorValues
x = map_list_of<Key, Vector>
(1, (Vec(1) << 2.))
(2, (Vec(1) << 5.)),
(1, (Vector(1) << 2.))
(2, (Vector(1) << 5.)),
y = map_list_of<Key, Vector>
(1, (Vec(1) << 2.))
(2, (Vec(1) << 3.));
(1, (Vector(1) << 2.))
(2, (Vector(1) << 3.));
// test functional version
VectorValues actual = cbn.backSubstituteTranspose(x);

4
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -25,11 +25,11 @@ using namespace std;
/* ************************************************************************* */
TEST(GaussianDensity, constructor)
{
Matrix R = (Mat(2,2) <<
Matrix R = (Matrix(2,2) <<
-12.1244, -5.1962,
0., 4.6904);
Vector d = (Vec(2) << 1.0, 2.0), s = (Vec(2) << 3.0, 4.0);
Vector d = (Vector(2) << 1.0, 2.0), s = (Vector(2) << 3.0, 4.0);
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
GaussianDensity copied(conditional);

80
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -46,15 +46,15 @@ TEST(GaussianFactorGraph, initialization) {
fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2);
JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
// Note that this the augmented vector and the RHS is in column 7
Matrix expectedIJS = (Mat(3, 22) <<
Matrix expectedIJS = (Matrix(3, 22) <<
1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8.,
1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5
@ -73,7 +73,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
Matrix expectedT = (Mat(16, 3) <<
Matrix expectedT = (Matrix(16, 3) <<
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
@ -95,8 +95,8 @@ TEST(GaussianFactorGraph, sparseJacobian) {
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, (Mat(2, 3) << 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model);
gfg.add(0, (Mat(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Mat(2, 2) << 11., 12., 14., 15.), (Vec(2) << 13.,16.), model);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model);
Matrix actual = gfg.sparseJacobian_();
@ -114,8 +114,8 @@ TEST(GaussianFactorGraph, matrices) {
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, (Mat(2, 3) << 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model);
gfg.add(0, (Mat(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Mat(2, 2) << 11., 12., 14., 15.), (Vec(2) << 13.,16.), model);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model);
Matrix jacobian(4,6);
jacobian <<
@ -151,11 +151,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2);
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2);
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2);
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
return fg;
}
@ -168,9 +168,9 @@ TEST( GaussianFactorGraph, gradient )
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
VectorValues expected = map_list_of<Key, Vector>
(1, (Vec(2) << 5.0, -12.5))
(2, (Vec(2) << 30.0, 5.0))
(0, (Vec(2) << -25.0, 17.5));
(1, (Vector(2) << 5.0, -12.5))
(2, (Vector(2) << 30.0, 5.0))
(0, (Vector(2) << -25.0, 17.5));
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
@ -190,15 +190,15 @@ TEST( GaussianFactorGraph, transposeMultiplication )
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
Errors e; e +=
(Vec(2) << 0.0, 0.0),
(Vec(2) << 15.0, 0.0),
(Vec(2) << 0.0,-5.0),
(Vec(2) << -7.5,-5.0);
(Vector(2) << 0.0, 0.0),
(Vector(2) << 15.0, 0.0),
(Vector(2) << 0.0,-5.0),
(Vector(2) << -7.5,-5.0);
VectorValues expected;
expected.insert(1, (Vec(2) << -37.5,-50.0));
expected.insert(2, (Vec(2) << -150.0, 25.0));
expected.insert(0, (Vec(2) << 187.5, 25.0));
expected.insert(1, (Vector(2) << -37.5,-50.0));
expected.insert(2, (Vector(2) << -150.0, 25.0));
expected.insert(0, (Vector(2) << 187.5, 25.0));
VectorValues actual = A.transposeMultiply(e);
EXPECT(assert_equal(expected, actual));
@ -242,14 +242,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
VectorValues x = map_list_of<Key, Vector>
(0, (Vec(2) << 1,2))
(1, (Vec(2) << 3,4))
(2, (Vec(2) << 5,6));
(0, (Vector(2) << 1,2))
(1, (Vector(2) << 3,4))
(2, (Vector(2) << 5,6));
VectorValues expected;
expected.insert(0, (Vec(2) << -450, -450));
expected.insert(1, (Vec(2) << 0, 0));
expected.insert(2, (Vec(2) << 950, 1050));
expected.insert(0, (Vector(2) << -450, -450));
expected.insert(1, (Vector(2) << 0, 0));
expected.insert(2, (Vector(2) << 950, 1050));
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
@ -263,8 +263,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vec(2) << 0.0, 1.0),
400*eye(2,2), (Vec(2) << 1.0, 1.0), 3.0);
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vector(2) << 0.0, 1.0),
400*eye(2,2), (Vector(2) << 1.0, 1.0), 3.0);
return gfg;
}
@ -280,14 +280,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
EXPECT(assert_equal(Y,AtA*X));
VectorValues x = map_list_of<Key, Vector>
(0, (Vec(2) << 1,2))
(1, (Vec(2) << 3,4))
(2, (Vec(2) << 5,6));
(0, (Vector(2) << 1,2))
(1, (Vector(2) << 3,4))
(2, (Vector(2) << 5,6));
VectorValues expected;
expected.insert(0, (Vec(2) << -450, -450));
expected.insert(1, (Vec(2) << 300, 400));
expected.insert(2, (Vec(2) << 2950, 3450));
expected.insert(0, (Vector(2) << -450, -450));
expected.insert(1, (Vector(2) << 300, 400));
expected.insert(2, (Vector(2) << 2950, 3450));
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
@ -306,7 +306,7 @@ TEST( GaussianFactorGraph, matricesMixed )
Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect !
Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct
EXPECT(assert_equal(A.transpose()*A, AtA));
Vector expected = - (Vec(6) << -25, 17.5, 5, -13.5, 29, 4);
Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4);
EXPECT(assert_equal(expected, eta));
EXPECT(assert_equal(A.transpose()*b, eta));
}
@ -318,9 +318,9 @@ TEST( GaussianFactorGraph, gradientAtZero )
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
VectorValues actual = gfg.gradientAtZero();
expected.insert(0, (Vec(2) << -25, 17.5));
expected.insert(1, (Vec(2) << 5, -13.5));
expected.insert(2, (Vec(2) << 29, 4));
expected.insert(0, (Vector(2) << -25, 17.5));
expected.insert(1, (Vector(2) << 5, -13.5));
expected.insert(2, (Vector(2) << 29, 4));
EXPECT(assert_equal(expected, actual));
}

142
gtsam/linear/tests/testHessianFactorUnordered.cpp
View File

@ -51,7 +51,7 @@ TEST(HessianFactor, emptyConstructor)
TEST(HessianFactor, ConversionConstructor)
{
HessianFactor expected(list_of(0)(1),
SymmetricBlockMatrix(list_of(2)(4)(1), (Mat(7,7) <<
SymmetricBlockMatrix(list_of(2)(4)(1), (Matrix(7,7) <<
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000,
@ -61,22 +61,22 @@ TEST(HessianFactor, ConversionConstructor)
25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500)));
JacobianFactor jacobian(
0, (Mat(4,2) << -1., 0.,
0, (Matrix(4,2) << -1., 0.,
+0.,-1.,
1., 0.,
+0.,1.),
1, (Mat(4,4) << 1., 0., 0.00, 0., // f4
1, (Matrix(4,4) << 1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00, -1.), // f2
(Vec(4) << -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas((Vec(4) << 0.2, 0.2, 0.1, 0.1)));
(Vector(4) << -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1)));
HessianFactor actual(jacobian);
VectorValues values = pair_list_of<Key, Vector>
(0, (Vec(2) << 1.0, 2.0))
(1, (Vec(4) << 3.0, 4.0, 5.0, 6.0));
(0, (Vector(2) << 1.0, 2.0))
(1, (Vector(4) << 3.0, 4.0, 5.0, 6.0));
EXPECT_LONGS_EQUAL(2, (long)actual.size());
EXPECT(assert_equal(expected, actual, 1e-9));
@ -86,8 +86,8 @@ TEST(HessianFactor, ConversionConstructor)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1)
{
Matrix G = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g = (Vec(2) << -8.0, -9.0);
Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g = (Vector(2) << -8.0, -9.0);
double f = 10.0;
HessianFactor factor(0, G, g, f);
@ -97,7 +97,7 @@ TEST(HessianFactor, Constructor1)
EXPECT(assert_equal(g, Vector(factor.linearTerm())));
EXPECT_LONGS_EQUAL(1, (long)factor.size());
VectorValues dx = pair_list_of<Key, Vector>(0, (Vec(2) << 1.5, 2.5));
VectorValues dx = pair_list_of<Key, Vector>(0, (Vector(2) << 1.5, 2.5));
// error 0.5*(f - 2*x'*g + x'*G*x)
double expected = 80.375;
@ -111,7 +111,7 @@ TEST(HessianFactor, Constructor1)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1b)
{
Vector mu = (Vec(2) << 1.0,2.0);
Vector mu = (Vector(2) << 1.0,2.0);
Matrix Sigma = eye(2,2);
HessianFactor factor(0, mu, Sigma);
@ -130,15 +130,15 @@ TEST(HessianFactor, Constructor1b)
/* ************************************************************************* */
TEST(HessianFactor, Constructor2)
{
Matrix G11 = (Mat(1,1) << 1.0);
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Matrix G11 = (Matrix(1,1) << 1.0);
Matrix G12 = (Matrix(1,2) << 2.0, 4.0);
Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
double f = 10.0;
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -164,31 +164,31 @@ TEST(HessianFactor, Constructor2)
VectorValues dxLarge = pair_list_of<Key, Vector>
(0, dx0)
(1, dx1)
(2, (Vec(2) << 0.1, 0.2));
(2, (Vector(2) << 0.1, 0.2));
EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
}
/* ************************************************************************* */
TEST(HessianFactor, Constructor3)
{
Matrix G11 = (Mat(1,1) << 1.0);
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0);
Matrix G11 = (Matrix(1,1) << 1.0);
Matrix G12 = (Matrix(1,2) << 2.0, 4.0);
Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -218,24 +218,24 @@ TEST(HessianFactor, Constructor3)
/* ************************************************************************* */
TEST(HessianFactor, ConstructorNWay)
{
Matrix G11 = (Mat(1,1) << 1.0);
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0);
Matrix G11 = (Matrix(1,1) << 1.0);
Matrix G12 = (Matrix(1,2) << 2.0, 4.0);
Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -271,30 +271,30 @@ TEST(HessianFactor, ConstructorNWay)
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate)
{
Matrix A01 = (Mat(3,3) <<
Matrix A01 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Matrix A10 = (Mat(3,3) <<
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = (Mat(3,3) <<
Matrix A11 = (Matrix(3,3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Matrix A21 = (Mat(3,3) <<
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -330,10 +330,10 @@ TEST(HessianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = (Mat(4,2) <<
Matrix Ax2 = (Matrix(4,2) <<
// x2
-1., 0.,
+0.,-1.,
@ -341,7 +341,7 @@ TEST(HessianFactor, eliminate2 )
+0.,1.
);
Matrix Al1x1 = (Mat(4,4) <<
Matrix Al1x1 = (Matrix(4,4) <<
// l1 x1
1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
@ -370,26 +370,26 @@ TEST(HessianFactor, eliminate2 )
// create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
Matrix R11 = (Mat(2,2) <<
Matrix R11 = (Matrix(2,2) <<
1.00, 0.00,
0.00, 1.00
)/oldSigma;
Matrix S12 = (Mat(2,4) <<
Matrix S12 = (Matrix(2,4) <<
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = (Vec(2) << 0.2,-0.14)/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(0, d, R11, 1, S12);
EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1x1 = (Mat(2,4) <<
Matrix Bl1x1 = (Matrix(2,4) <<
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = (Vec(2) << 0.0,0.894427);
Vector b1 = (Vector(2) << 0.0,0.894427);
JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
}
@ -398,16 +398,16 @@ TEST(HessianFactor, eliminate2 )
TEST(HessianFactor, combine) {
// update the information matrix with a single jacobian factor
Matrix A0 = (Mat(2, 2) <<
Matrix A0 = (Matrix(2, 2) <<
11.1803399, 0.0,
0.0, 11.1803399);
Matrix A1 = (Mat(2, 2) <<
Matrix A1 = (Matrix(2, 2) <<
-2.23606798, 0.0,
0.0, -2.23606798);
Matrix A2 = (Mat(2, 2) <<
Matrix A2 = (Matrix(2, 2) <<
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = (Vec(2) << 2.23606798,-1.56524758);
Vector b = (Vector(2) << 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
GaussianFactorGraph factors = list_of(f);
@ -415,7 +415,7 @@ TEST(HessianFactor, combine) {
// Form Ab' * Ab
HessianFactor actual(factors);
Matrix expected = (Mat(7, 7) <<
Matrix expected = (Matrix(7, 7) <<
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
@ -430,11 +430,11 @@ TEST(HessianFactor, combine) {
/* ************************************************************************* */
TEST(HessianFactor, gradientAtZero)
{
Matrix G11 = (Mat(1, 1) << 1.0);
Matrix G12 = (Mat(1, 2) << 0.0, 0.0);
Matrix G22 = (Mat(2, 2) << 1.0, 0.0, 0.0, 1.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Matrix G11 = (Matrix(1, 1) << 1.0);
Matrix G12 = (Matrix(1, 2) << 0.0, 0.0);
Matrix G22 = (Matrix(2, 2) << 1.0, 0.0, 0.0, 1.0);
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
double f = 194;
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);

70
gtsam/linear/tests/testJacobianFactorUnordered.cpp
View File

@ -42,8 +42,8 @@ namespace {
(make_pair(15, 3*Matrix3::Identity()));
// RHS and sigmas
const Vector b = (Vec(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.5));
const Vector b = (Vector(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5));
}
}
@ -240,22 +240,22 @@ TEST(JacobianFactor, operators )
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Vector b = (Vec(2) << 0.2,-0.1);
Vector b = (Vector(2) << 0.2,-0.1);
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
VectorValues c;
c.insert(1, (Vec(2) << 10.,20.));
c.insert(2, (Vec(2) << 30.,60.));
c.insert(1, (Vector(2) << 10.,20.));
c.insert(2, (Vector(2) << 30.,60.));
// test A*x
Vector expectedE = (Vec(2) << 200.,400.);
Vector expectedE = (Vector(2) << 200.,400.);
Vector actualE = lf * c;
EXPECT(assert_equal(expectedE, actualE));
// test A^e
VectorValues expectedX;
expectedX.insert(1, (Vec(2) << -2000.,-4000.));
expectedX.insert(2, (Vec(2) << 2000., 4000.));
expectedX.insert(1, (Vector(2) << -2000.,-4000.));
expectedX.insert(2, (Vector(2) << 2000., 4000.));
VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX));
@ -263,8 +263,8 @@ TEST(JacobianFactor, operators )
// test gradient at zero
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
VectorValues expectedG;
expectedG.insert(1, (Vec(2) << 20,-10));
expectedG.insert(2, (Vec(2) << -20, 10));
expectedG.insert(1, (Vector(2) << 20,-10));
expectedG.insert(2, (Vector(2) << -20, 10));
FastVector<Key> keys; keys += 1,2;
EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
VectorValues actualG = lf.gradientAtZero();
@ -290,30 +290,30 @@ TEST(JacobianFactor, empty )
/* ************************************************************************* */
TEST(JacobianFactor, eliminate)
{
Matrix A01 = (Mat(3, 3) <<
Matrix A01 = (Matrix(3, 3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Matrix A10 = (Mat(3, 3) <<
Matrix A10 = (Matrix(3, 3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = (Mat(3, 3) <<
Matrix A11 = (Matrix(3, 3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Matrix A21 = (Mat(3, 3) <<
Matrix A21 = (Matrix(3, 3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -345,10 +345,10 @@ TEST(JacobianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = (Mat(4, 2) <<
Matrix Ax2 = (Matrix(4, 2) <<
// x2
-1., 0.,
+0.,-1.,
@ -356,7 +356,7 @@ TEST(JacobianFactor, eliminate2 )
+0.,1.
);
Matrix Al1x1 = (Mat(4, 4) <<
Matrix Al1x1 = (Matrix(4, 4) <<
// l1 x1
1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
@ -382,27 +382,27 @@ TEST(JacobianFactor, eliminate2 )
// create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
Matrix R11 = (Mat(2, 2) <<
Matrix R11 = (Matrix(2, 2) <<
1.00, 0.00,
0.00, 1.00
)/oldSigma;
Matrix S12 = (Mat(2, 4) <<
Matrix S12 = (Matrix(2, 4) <<
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = (Vec(2) << 0.2,-0.14)/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(2, d, R11, 11, S12);
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1x1 = (Mat(2, 4) <<
Matrix Bl1x1 = (Matrix(2, 4) <<
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = (Vec(2) << 0.0, 0.894427);
Vector b1 = (Vector(2) << 0.0, 0.894427);
JacobianFactor expectedLF(11, Bl1x1, b1);
EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
}
@ -411,7 +411,7 @@ TEST(JacobianFactor, eliminate2 )
TEST(JacobianFactor, EliminateQR)
{
// Augmented Ab test case for whole factor graph
Matrix Ab = (Mat(14, 11) <<
Matrix Ab = (Matrix(14, 11) <<
4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1.,
9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4.,
5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0.,
@ -441,7 +441,7 @@ TEST(JacobianFactor, EliminateQR)
EXPECT(assert_equal(2.0 * Ab, actualDense));
// Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
Matrix R = 2.0 * (Mat(11, 11) <<
Matrix R = 2.0 * (Matrix(11, 11) <<
-12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
@ -486,7 +486,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
EXPECT(actual.second->size() == 0);
// verify conditional Gaussian
Vector sigmas = (Vec(2) << 0.0, 0.0);
Vector sigmas = (Vector(2) << 0.0, 0.0);
GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first));
}
@ -523,14 +523,14 @@ TEST ( JacobianFactor, constraint_eliminate2 )
EXPECT(assert_equal(expectedLF, *actual.second));
// verify CG
Matrix R = (Mat(2, 2) <<
Matrix R = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 1.0);
Matrix S = (Mat(2, 2) <<
Matrix S = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 0.0);
Vector d = (Vec(2) << 3.0, 0.6666);
Vector sigmas = (Vec(2) << 0.0, 0.0);
Vector d = (Vector(2) << 3.0, 0.6666);
Vector sigmas = (Vector(2) << 0.0, 0.0);
GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
}

34
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
/** Small 2D point class implemented as a Vector */
struct State: Vector {
State(double x, double y) :
Vector((Vec(2) << x, y)) {
Vector((Vector(2) << x, y)) {
}
};
@ -49,7 +49,7 @@ TEST( KalmanFilter, constructor ) {
// Assert it has the correct mean, covariance and information
EXPECT(assert_equal(x_initial, p1->mean()));
Matrix Sigma = (Mat(2, 2) << 0.01, 0.0, 0.0, 0.01);
Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01);
EXPECT(assert_equal(Sigma, p1->covariance()));
EXPECT(assert_equal(inverse(Sigma), p1->information()));
@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) {
// Create the controls and measurement properties for our example
Matrix F = eye(2, 2);
Matrix B = eye(2, 2);
Vector u = (Vec(2) << 1.0, 0.0);
Vector u = (Vector(2) << 1.0, 0.0);
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01*eye(2, 2);
Matrix H = eye(2, 2);
@ -135,10 +135,10 @@ TEST( KalmanFilter, linear1 ) {
TEST( KalmanFilter, predict ) {
// Create dynamics model
Matrix F = (Mat(2, 2) << 1.0, 0.1, 0.2, 1.1);
Matrix B = (Mat(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8);
Vector u = (Vec(3) << 1.0, 0.0, 2.0);
Matrix R = (Mat(2, 2) << 1.0, 0.5, 0.0, 3.0);
Matrix F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1);
Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8);
Vector u = (Vector(3) << 1.0, 0.0, 2.0);
Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0);
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
@ -168,7 +168,7 @@ TEST( KalmanFilter, predict ) {
TEST( KalmanFilter, QRvsCholesky ) {
Vector mean = ones(9);
Matrix covariance = 1e-6 * (Mat(9, 9) <<
Matrix covariance = 1e-6 * (Matrix(9, 9) <<
15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6,
-6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1,
0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0,
@ -187,7 +187,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
KalmanFilter::State p0b = kfb.init(mean, covariance);
// Set up dynamics update
Matrix Psi_k = 1e-6 * (Mat(9, 9) <<
Matrix Psi_k = 1e-6 * (Matrix(9, 9) <<
1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0,
0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0,
@ -199,7 +199,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0);
Matrix B = zeros(9, 1);
Vector u = zero(1);
Matrix dt_Q_k = 1e-6 * (Mat(9, 9) <<
Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@ -219,10 +219,10 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa->information(), pb->information(), 1e-7));
// and in addition attain the correct covariance
Vector expectedMean = (Vec(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.);
Vector expectedMean = (Vector(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.);
EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7));
EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7));
Matrix expected = 1e-6 * (Mat(9, 9) <<
Matrix expected = 1e-6 * (Matrix(9, 9) <<
48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6,
-3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1,
-0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0,
@ -236,12 +236,12 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(expected, pb->covariance(), 1e-7));
// prepare update
Matrix H = 1e-3 * (Mat(3, 9) <<
Matrix H = 1e-3 * (Matrix(3, 9) <<
0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0,
-9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0,
-83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.);
Vector z = (Vec(3) << 0.2599 , 1.3327 , 0.2007);
Vector sigmas = (Vec(3) << 0.3323 , 0.2470 , 0.1904);
Vector z = (Vector(3) << 0.2599 , 1.3327 , 0.2007);
Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904);
SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
// do update
@ -253,10 +253,10 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7));
// and in addition attain the correct mean and covariance
Vector expectedMean2 = (Vec(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882);
Vector expectedMean2 = (Vector(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882);
EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here !
EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss?
Matrix expected2 = 1e-6 * (Mat(9, 9) <<
Matrix expected2 = 1e-6 * (Matrix(9, 9) <<
46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5,
-2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1,
-0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0,

70
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -31,11 +31,11 @@ using namespace gtsam;
using namespace noiseModel;
static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
static Matrix R = (Mat(3, 3) <<
static Matrix R = (Matrix(3, 3) <<
s_1, 0.0, 0.0,
0.0, s_1, 0.0,
0.0, 0.0, s_1);
static Matrix Sigma = (Mat(3, 3) <<
static Matrix Sigma = (Matrix(3, 3) <<
var, 0.0, 0.0,
0.0, var, 0.0,
0.0, 0.0, var);
@ -45,17 +45,17 @@ static Matrix Sigma = (Mat(3, 3) <<
/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
Vector whitened = (Vec(3) << 5.0,10.0,15.0);
Vector unwhitened = (Vec(3) << 10.0,20.0,30.0);
Vector whitened = (Vector(3) << 5.0,10.0,15.0);
Vector unwhitened = (Vector(3) << 10.0,20.0,30.0);
// Construct noise models
vector<Gaussian::shared_ptr> m;
m.push_back(Gaussian::SqrtInformation(R));
m.push_back(Gaussian::Covariance(Sigma));
//m.push_back(Gaussian::Information(Q));
m.push_back(Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)));
m.push_back(Diagonal::Variances((Vec(3) << var, var, var)));
m.push_back(Diagonal::Precisions((Vec(3) << prc, prc, prc)));
m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)));
m.push_back(Diagonal::Variances((Vector(3) << var, var, var)));
m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc)));
m.push_back(Isotropic::Sigma(3, sigma));
m.push_back(Isotropic::Variance(3, var));
m.push_back(Isotropic::Precision(3, prc));
@ -74,7 +74,7 @@ TEST(NoiseModel, constructors)
DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
// test R matrix
Matrix expectedR((Mat(3, 3) <<
Matrix expectedR((Matrix(3, 3) <<
s_1, 0.0, 0.0,
0.0, s_1, 0.0,
0.0, 0.0, s_1));
@ -83,12 +83,12 @@ TEST(NoiseModel, constructors)
EXPECT(assert_equal(expectedR,mi->R()));
// test Whiten operator
Matrix H((Mat(3, 4) <<
Matrix H((Matrix(3, 4) <<
0.0, 0.0, 1.0, 1.0,
0.0, 1.0, 0.0, 1.0,
1.0, 0.0, 0.0, 1.0));
Matrix expected((Mat(3, 4) <<
Matrix expected((Matrix(3, 4) <<
0.0, 0.0, s_1, s_1,
0.0, s_1, 0.0, s_1,
s_1, 0.0, 0.0, s_1));
@ -104,7 +104,7 @@ TEST(NoiseModel, constructors)
/* ************************************************************************* */
TEST(NoiseModel, Unit)
{
Vector v = (Vec(3) << 5.0,10.0,15.0);
Vector v = (Vector(3) << 5.0,10.0,15.0);
Gaussian::shared_ptr u(Unit::Create(3));
EXPECT(assert_equal(v,u->whiten(v)));
}
@ -114,8 +114,8 @@ TEST(NoiseModel, equals)
{
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)),
d2 = Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)),
d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
i2 = Isotropic::Sigma(3, 0.7);
@ -133,7 +133,7 @@ TEST(NoiseModel, equals)
///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart )
//{
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma), true);
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true);
// Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
// EXPECT(n1);
@ -152,8 +152,8 @@ TEST(NoiseModel, ConstrainedConstructors )
Constrained::shared_ptr actual;
size_t d = 3;
double m = 100.0;
Vector sigmas = (Vec(3) << sigma, 0.0, 0.0);
Vector mu = (Vec(3) << 200.0, 300.0, 400.0);
Vector sigmas = (Vector(3) << sigma, 0.0, 0.0);
Vector mu = (Vector(3) << 200.0, 300.0, 400.0);
actual = Constrained::All(d);
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
@ -173,12 +173,12 @@ TEST(NoiseModel, ConstrainedConstructors )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
Vector feasible = (Vec(3) << 1.0, 0.0, 1.0),
infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma));
Vector feasible = (Vector(3) << 1.0, 0.0, 1.0),
infeasible = (Vector(3) << 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma));
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal((Vec(3) << 0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal((Vec(3) << 0.5, 0.0, 0.5),d->whiten(feasible)));
EXPECT(assert_equal((Vector(3) << 0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal((Vector(3) << 0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
@ -187,13 +187,13 @@ TEST(NoiseModel, ConstrainedMixed )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll )
{
Vector feasible = (Vec(3) << 0.0, 0.0, 0.0),
infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Vector feasible = (Vector(3) << 0.0, 0.0, 0.0),
infeasible = (Vector(3) << 1.0, 1.0, 1.0);
Constrained::shared_ptr i = Constrained::All(3);
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal((Vec(3) << 1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal((Vec(3) << 0.0, 0.0, 0.0),i->whiten(feasible)));
EXPECT(assert_equal((Vector(3) << 1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal((Vector(3) << 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
@ -202,15 +202,15 @@ TEST(NoiseModel, ConstrainedAll )
/* ************************************************************************* */
namespace exampleQR {
// create a matrix to eliminate
Matrix Ab = (Mat(4, 7) <<
Matrix Ab = (Matrix(4, 7) <<
-1., 0., 1., 0., 0., 0., -0.2,
0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1);
Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1);
// the matrix AB yields the following factorized version:
Matrix Rd = (Mat(4, 7) <<
Matrix Rd = (Matrix(4, 7) <<
11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525,
0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0,
@ -225,7 +225,7 @@ TEST( NoiseModel, QR )
Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
// Expected result
Vector expectedSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Gaussian version
@ -238,7 +238,7 @@ TEST( NoiseModel, QR )
SharedDiagonal actual2 = constrained->QR(Ab2);
SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas);
EXPECT(assert_equal(*expectedModel2,*actual2,1e-6));
Matrix expectedRd2 = (Mat(4, 7) <<
Matrix expectedRd2 = (Matrix(4, 7) <<
1., 0., -0.2, 0., -0.8, 0., 0.2,
0., 1., 0.,-0.2, 0., -0.8,-0.14,
0., 0., 1., 0., -1., 0., 0.0,
@ -250,10 +250,10 @@ TEST( NoiseModel, QR )
TEST(NoiseModel, QRNan )
{
SharedDiagonal constrained = noiseModel::Constrained::All(2);
Matrix Ab = (Mat(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);
Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);
SharedDiagonal expected = noiseModel::Constrained::All(2);
Matrix expectedAb = (Mat(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expected,*actual));
@ -281,7 +281,7 @@ TEST(NoiseModel, ScalarOrVector )
/* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace)
{
Vector sigmas = (Vec(3) << 0.1, 0.1, 0.1);
Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1);
SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3);
model->WhitenInPlace(A);
@ -304,8 +304,8 @@ TEST(NoiseModel, robustFunction)
TEST(NoiseModel, robustNoise)
{
const double k = 10.0, error1 = 1.0, error2 = 100.0;
Matrix A = (Mat(2, 2) << 1.0, 10.0, 100.0, 1000.0);
Vector b = (Vec(2) << error1, error2);
Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0);
Vector b = (Vector(2) << error1, error2);
const Robust::shared_ptr robust = Robust::Create(
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
Unit::Create(2));

34
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -49,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vec(3) << 0.0, 0.0, 0.1));
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vector(3) << 0.0, 0.0, 0.1));
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
@ -135,9 +135,9 @@ BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
VectorValues values;
values.insert(0, (Vec(1) << 1.0));
values.insert(1, (Vec(2) << 2.0,3.0));
values.insert(2, (Vec(2) << 4.0,5.0));
values.insert(0, (Vector(1) << 1.0));
values.insert(1, (Vector(2) << 2.0,3.0));
values.insert(2, (Vector(2) << 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
EXPECT(equalsBinary<VectorValues>(values));
@ -145,7 +145,7 @@ TEST (Serialization, linear_factors) {
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor));
@ -159,9 +159,9 @@ TEST (Serialization, linear_factors) {
/* ************************************************************************* */
TEST (Serialization, gaussian_conditional) {
Matrix A1 = (Mat(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Mat(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Mat(2, 2) << 0.1, 0.3, 0.0, 0.34);
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2);
@ -174,9 +174,9 @@ TEST (Serialization, gaussian_conditional) {
TEST (Serialization, gaussian_factor_graph) {
GaussianFactorGraph graph;
{
Matrix A1 = (Mat(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Mat(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Mat(2, 2) << 0.1, 0.3, 0.0, 0.34);
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2);
graph.push_back(cg);
@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) {
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
HessianFactor hessianfactor(jacobianfactor);
graph.push_back(jacobianfactor);
@ -203,10 +203,10 @@ TEST (Serialization, gaussian_bayes_tree) {
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, (Mat(1, 1) << 1.), x1, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, (Mat(1, 1) << 1.), x3, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x3, (Mat(1, 1) << 1.), x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise));
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);

78
gtsam/linear/tests/testVectorValuesUnordered.cpp
View File

@ -36,10 +36,10 @@ TEST(VectorValues, basics)
// insert
VectorValues actual;
actual.insert(0, (Vec(1) << 1));
actual.insert(1, (Vec(2) << 2, 3));
actual.insert(5, (Vec(2) << 6, 7));
actual.insert(2, (Vec(2) << 4, 5));
actual.insert(0, (Vector(1) << 1));
actual.insert(1, (Vector(2) << 2, 3));
actual.insert(5, (Vector(2) << 6, 7));
actual.insert(2, (Vector(2) << 4, 5));
// Check dimensions
LONGS_EQUAL(4, actual.size());
@ -58,12 +58,12 @@ TEST(VectorValues, basics)
EXPECT(!actual.exists(6));
// Check values
EXPECT(assert_equal((Vec(1) << 1), actual[0]));
EXPECT(assert_equal((Vec(2) << 2, 3), actual[1]));
EXPECT(assert_equal((Vec(2) << 4, 5), actual[2]));
EXPECT(assert_equal((Vec(2) << 6, 7), actual[5]));
EXPECT(assert_equal((Vector(1) << 1), actual[0]));
EXPECT(assert_equal((Vector(2) << 2, 3), actual[1]));
EXPECT(assert_equal((Vector(2) << 4, 5), actual[2]));
EXPECT(assert_equal((Vector(2) << 6, 7), actual[5]));
FastVector<Key> keys = list_of(0)(1)(2)(5);
EXPECT(assert_equal((Vec(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@ -74,18 +74,18 @@ TEST(VectorValues, basics)
TEST(VectorValues, combine)
{
VectorValues expected;
expected.insert(0, (Vec(1) << 1));
expected.insert(1, (Vec(2) << 2, 3));
expected.insert(5, (Vec(2) << 6, 7));
expected.insert(2, (Vec(2) << 4, 5));
expected.insert(0, (Vector(1) << 1));
expected.insert(1, (Vector(2) << 2, 3));
expected.insert(5, (Vector(2) << 6, 7));
expected.insert(2, (Vector(2) << 4, 5));
VectorValues first;
first.insert(0, (Vec(1) << 1));
first.insert(1, (Vec(2) << 2, 3));
first.insert(0, (Vector(1) << 1));
first.insert(1, (Vector(2) << 2, 3));
VectorValues second;
second.insert(5, (Vec(2) << 6, 7));
second.insert(2, (Vec(2) << 4, 5));
second.insert(5, (Vector(2) << 6, 7));
second.insert(2, (Vector(2) << 4, 5));
VectorValues actual(first, second);
@ -96,14 +96,14 @@ TEST(VectorValues, combine)
TEST(VectorValues, subvector)
{
VectorValues init;
init.insert(10, (Vec(1) << 1));
init.insert(11, (Vec(2) << 2, 3));
init.insert(12, (Vec(2) << 4, 5));
init.insert(13, (Vec(2) << 6, 7));
init.insert(10, (Vector(1) << 1));
init.insert(11, (Vector(2) << 2, 3));
init.insert(12, (Vector(2) << 4, 5));
init.insert(13, (Vector(2) << 6, 7));
std::vector<Key> keys;
keys += 10, 12, 13;
Vector expSubVector = (Vec(5) << 1, 4, 5, 6, 7);
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7);
EXPECT(assert_equal(expSubVector, init.vector(keys)));
}
@ -111,16 +111,16 @@ TEST(VectorValues, subvector)
TEST(VectorValues, LinearAlgebra)
{
VectorValues test1;
test1.insert(0, (Vec(1) << 1));
test1.insert(1, (Vec(2) << 2, 3));
test1.insert(5, (Vec(2) << 6, 7));
test1.insert(2, (Vec(2) << 4, 5));
test1.insert(0, (Vector(1) << 1));
test1.insert(1, (Vector(2) << 2, 3));
test1.insert(5, (Vector(2) << 6, 7));
test1.insert(2, (Vector(2) << 4, 5));
VectorValues test2;
test2.insert(0, (Vec(1) << 6));
test2.insert(1, (Vec(2) << 1, 6));
test2.insert(5, (Vec(2) << 4, 3));
test2.insert(2, (Vec(2) << 1, 8));
test2.insert(0, (Vector(1) << 6));
test2.insert(1, (Vector(2) << 1, 6));
test2.insert(5, (Vector(2) << 4, 3));
test2.insert(2, (Vector(2) << 1, 8));
// Dot product
double dotExpected = test1.vector().dot(test2.vector());
@ -175,10 +175,10 @@ TEST(VectorValues, convert)
x << 1, 2, 3, 4, 5, 6, 7;
VectorValues expected;
expected.insert(0, (Vec(1) << 1));
expected.insert(1, (Vec(2) << 2, 3));
expected.insert(2, (Vec(2) << 4, 5));
expected.insert(5, (Vec(2) << 6, 7));
expected.insert(0, (Vector(1) << 1));
expected.insert(1, (Vector(2) << 2, 3));
expected.insert(2, (Vector(2) << 4, 5));
expected.insert(5, (Vector(2) << 6, 7));
std::map<Key,size_t> dims;
dims.insert(make_pair(0,1));
@ -200,11 +200,11 @@ TEST(VectorValues, convert)
TEST(VectorValues, vector_sub)
{
VectorValues vv;
vv.insert(0, (Vec(1) << 1));
vv.insert(1, (Vec(2) << 2, 3));
vv.insert(2, (Vec(2) << 4, 5));
vv.insert(5, (Vec(2) << 6, 7));
vv.insert(7, (Vec(2) << 8, 9));
vv.insert(0, (Vector(1) << 1));
vv.insert(1, (Vector(2) << 2, 3));
vv.insert(2, (Vector(2) << 4, 5));
vv.insert(5, (Vector(2) << 6, 7));
vv.insert(7, (Vector(2) << 8, 9));
std::map<Key,size_t> dims;
dims.insert(make_pair(0,1));

4
gtsam/navigation/tests/testImuBias.cpp
View File

@ -25,8 +25,8 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( ImuBias, Constructor)
{
Vector bias_acc((Vec(3) << 0.1,0.2,0.4));
Vector bias_gyro((Vec(3) << -0.2, 0.5, 0.03));
Vector bias_acc((Vector(3) << 0.1,0.2,0.4));
Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03));
// Default Constructor
gtsam::imuBias::ConstantBias bias1;

12
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -75,11 +75,11 @@ namespace gtsam {
Key j1, Key j2) {
double e = u - z, e2 = e * e;
double c = 2 * logSqrt2PI - log(p) + e2 * p;
Vector g1 = (Vec(1) << -e * p);
Vector g2 = (Vec(1) << 0.5 / p - 0.5 * e2);
Matrix G11 = (Mat(1, 1) << p);
Matrix G12 = (Mat(1, 1) << e);
Matrix G22 = (Mat(1, 1) << 0.5 / (p * p));
Vector g1 = (Vector(1) << -e * p);
Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2);
Matrix G11 = (Matrix(1, 1) << p);
Matrix G12 = (Matrix(1, 1) << e);
Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p));
return HessianFactor::shared_ptr(
new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
}
@ -137,7 +137,7 @@ namespace gtsam {
* TODO: Where is this used? should disappear.
*/
virtual Vector unwhitenedError(const Values& x) const {
return (Vec(1) << std::sqrt(2 * error(x)));
return (Vector(1) << std::sqrt(2 * error(x)));
}
/**

54
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -30,13 +30,13 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Matrix A1 = (Mat(2, 2) <<
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
0.0, 2.63624);
Matrix A2 = (Mat(2, 2) <<
Matrix A2 = (Matrix(2, 2) <<
-0.0455167, -0.0443573,
-0.0222154, -0.102489);
Vector b = (Vec(2) << 0.0277052,
Vector b = (Vector(2) << 0.0277052,
-0.0533393);
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
@ -64,13 +64,13 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Matrix A1 = (Mat(2, 2) <<
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
0.0, 2.63624);
Matrix A2 = (Mat(2, 2) <<
Matrix A2 = (Matrix(2, 2) <<
-0.0455167, -0.0443573,
-0.0222154, -0.102489);
Vector b = (Vec(2) << 0.0277052,
Vector b = (Vector(2) << 0.0277052,
-0.0533393);
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
@ -97,8 +97,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint()));
// Check error evaluation
Vector delta_l1 = (Vec(2) << 1.0, 2.0);
Vector delta_l2 = (Vec(2) << 3.0, 4.0);
Vector delta_l1 = (Vector(2) << 1.0, 2.0);
Vector delta_l2 = (Vector(2) << 3.0, 4.0);
VectorValues delta = values.zeroVectors();
delta.at(l1) = delta_l1;
@ -117,22 +117,22 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_hessian_factor ) {
Matrix G11 = (Mat(1, 1) << 1.0);
Matrix G12 = (Mat(1, 2) << 2.0, 4.0);
Matrix G13 = (Mat(1, 3) << 3.0, 6.0, 9.0);
Matrix G11 = (Matrix(1, 1) << 1.0);
Matrix G12 = (Matrix(1, 2) << 2.0, 4.0);
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0);
Matrix G22 = (Mat(2, 2) << 3.0, 5.0,
Matrix G22 = (Matrix(2, 2) << 3.0, 5.0,
0.0, 6.0);
Matrix G23 = (Mat(2, 3) << 4.0, 6.0, 8.0,
Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0,
1.0, 2.0, 4.0);
Matrix G33 = (Mat(3, 3) << 1.0, 2.0, 3.0,
Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0,
0.0, 5.0, 6.0,
0.0, 0.0, 9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
double f = 10.0;
@ -158,21 +158,21 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
// 2 variable example, one pose, one landmark (planar)
// Initial ordering: x1, l1
Matrix G11 = (Mat(3, 3) <<
Matrix G11 = (Matrix(3, 3) <<
1.0, 2.0, 3.0,
0.0, 5.0, 6.0,
0.0, 0.0, 9.0);
Matrix G12 = (Mat(3, 2) <<
Matrix G12 = (Matrix(3, 2) <<
1.0, 2.0,
3.0, 5.0,
4.0, 6.0);
Vector g1 = (Vec(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(3) << 1.0, 2.0, 3.0);
Matrix G22 = (Mat(2, 2) <<
Matrix G22 = (Matrix(2, 2) <<
0.5, 0.2,
0.0, 0.6);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
double f = 10.0;
@ -197,16 +197,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
EXPECT(assert_equal(expLinPoints, actLinPoint));
// Create delta
Vector delta_l1 = (Vec(2) << 1.0, 2.0);
Vector delta_x1 = (Vec(3) << 3.0, 4.0, 0.5);
Vector delta_x2 = (Vec(3) << 6.0, 7.0, 0.3);
Vector delta_l1 = (Vector(2) << 1.0, 2.0);
Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5);
Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3);
// Check error calculation
VectorValues delta = linearizationPoint.zeroVectors();
delta.at(l1) = delta_l1;
delta.at(x1) = delta_x1;
delta.at(x2) = delta_x2;
EXPECT(assert_equal((Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys())));
EXPECT(assert_equal((Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys())));
Values noisyValues = linearizationPoint.retract(delta);
double expError = initFactor.error(delta);
@ -214,7 +214,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol);
// Compute updated versions
Vector dv = (Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0);
Vector dv = (Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0);
Vector g(5); g << g1, g2;
Vector g_prime = g - G.selfadjointView<Eigen::Upper>() * dv;

16
gtsam/nonlinear/tests/testValues.cpp
View File

@ -171,8 +171,8 @@ TEST(Values, expmap_a)
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vec(3) << 1.0, 1.1, 1.2))
(key2, (Vec(3) << 1.3, 1.4, 1.5));
(key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
@ -189,7 +189,7 @@ TEST(Values, expmap_b)
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vec(3) << 1.3, 1.4, 1.5));
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
@ -242,8 +242,8 @@ TEST(Values, localCoordinates)
valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vec(3) << 0.1, 0.2, 0.3))
(key2, (Vec(3) << 0.4, 0.5, 0.6));
(key1, (Vector(3) << 0.1, 0.2, 0.3))
(key2, (Vector(3) << 0.4, 0.5, 0.6));
Values valuesB = valuesA.retract(expDelta);
@ -275,11 +275,11 @@ TEST(Values, extract_keys)
TEST(Values, exists_)
{
Values config0;
config0.insert(key1, LieVector((Vec(1) << 1.)));
config0.insert(key2, LieVector((Vec(1) << 2.)));
config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector((Vector(1) << 2.)));
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
CHECK(assert_equal((Vec(1) << 1.),*v));
CHECK(assert_equal((Vector(1) << 1.),*v));
}
/* ************************************************************************* */

8
gtsam/slam/BoundingConstraint.h
View File

@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
}
if (isGreaterThan_)
return (Vec(1) << error);
return (Vector(1) << error);
else
return -1.0 * (Vec(1) << error);
return -1.0 * (Vector(1) << error);
}
private:
@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
}
if (isGreaterThan_)
return (Vec(1) << error);
return (Vector(1) << error);
else
return -1.0 * (Vec(1) << error);
return -1.0 * (Vector(1) << error);
}
private:

8
gtsam/slam/dataset.cpp
View File

@ -151,7 +151,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) {
Vector variances = (Vec(3) << m(0, 0), m(1, 1), m(2, 2));
Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
model = noiseModel::Diagonal::Variances(variances, smart);
}
@ -179,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
continue;
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std));
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet
@ -211,7 +211,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
{
double rangeVar = v1;
double bearingVar = v1 / 10.0;
measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << bearingVar, rangeVar));
measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar));
}
else
{
@ -386,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
continue;
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std));
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet

6
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = (Vec(2) << 323.,240.);
Vector z = (Vector(2) << 323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;
@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
}
else {
Vector delta = (Vec(11) <<
Vector delta = (Vector(11) <<
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor_Cal3Bundler, equals )
{
// Create two identical factors and make sure they're equal
Vector z = (Vec(2) << 323.,240.);
Vector z = (Vector(2) << 323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
@ -111,7 +111,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -308,7 +308,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
}
else {
Vector delta = (Vec(9) <<
Vector delta = (Vector(9) <<
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2

10
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -18,8 +18,8 @@
using namespace gtsam;
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vec(1) << 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
typedef PoseRotationPrior<Pose2> Pose2RotationPrior;
typedef PoseRotationPrior<Pose3> Pose3RotationPrior;
@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1;
// Pose3 examples
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vec(3) << 0.1, 0.2, 0.3));
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) << 0.1, 0.2, 0.3));
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
EXPECT(assert_equal((Vec(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal((Vec(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));

12
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -15,8 +15,8 @@
using namespace gtsam;
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior;
typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior;
@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3 pose1(rot3B, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3 pose1(rot3C, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal((Vec(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));

12
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -108,7 +108,7 @@ TEST( ProjectionFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vec(2) << -3.0, 0.0);
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -131,7 +131,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vec(2) << -3.0, 0.0);
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -154,8 +154,8 @@ TEST( ProjectionFactor, Jacobian ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = (Mat(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H2Expected = (Mat(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
@ -180,8 +180,8 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = (Mat(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H2Expected = (Mat(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));

8
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -118,7 +118,7 @@ TEST( RangeFactor, Error2D ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vec(1) << 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -143,7 +143,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vec(1) << 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -165,7 +165,7 @@ TEST( RangeFactor, Error3D ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vec(1) << 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -190,7 +190,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vec(1) << 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));

14
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -31,7 +31,7 @@
using namespace std;
using namespace gtsam;
static Pose3 camera1((Matrix) (Mat(3, 3) <<
static Pose3 camera1((Matrix) (Matrix(3, 3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0);
Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0);
Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -144,10 +144,10 @@ TEST( StereoFactor, Jacobian ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = (Mat(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
Matrix H1Expected = (Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
625.0, 0.0, 0.0, 0.0, -100.0, 0.0);
Matrix H2Expected = (Mat(3, 3) << 100.0, 0.0, 0.0,
Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0,
100.0, 0.0, 8.0,
0.0, 100.0, 0.0);
@ -172,10 +172,10 @@ TEST( StereoFactor, JacobianWithTransform ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = (Mat(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
Matrix H1Expected = (Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
-100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
-10.0, -650.0, 0.0, 0.0, 0.0, 100.0);
Matrix H2Expected = (Mat(3, 3) << 0.0, -100.0, 0.0,
Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0,
8.0, -100.0, 0.0,
0.0, 0.0, -100.0);

6
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -126,7 +126,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
const Velocity3& v1 = v();
// Update vehicle heading
Rot3 r2 = r1.retract((Vec(3) << 0.0, 0.0, heading_rate * dt));
Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt));
const double yaw2 = r2.ypr()(0);
// Update vehicle position
@ -150,7 +150,7 @@ PoseRTV PoseRTV::flyingDynamics(
const Velocity3& v1 = v();
// Update vehicle heading (and normalise yaw)
Vector rot_rates = (Vec(3) << 0.0, pitch_rate, heading_rate);
Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate);
Rot3 r2 = r1.retract(rot_rates*dt);
// Work out dynamics on platform
@ -165,7 +165,7 @@ PoseRTV PoseRTV::flyingDynamics(
Point3 forward(forward_accel, 0.0, 0.0);
Vector Acc_n =
yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame
- drag * (Vec(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1
- drag * (Vector(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1
+ delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch
// Update Vehicle Position and Velocity

4
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -126,7 +126,7 @@ public:
Matrix D_gravityBody_gk;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk);
Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
@ -154,7 +154,7 @@ public:
Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vec(3) << 0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0));
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vector(3) << 0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Unit::Create(6);
@ -103,9 +103,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0));
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

10
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -47,7 +47,7 @@ TEST( testPoseRTV, constructors ) {
EXPECT(assert_equal(Velocity3(), state4.v(), tol));
EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol));
Vector vec_init = (Vec(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6);
Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6);
PoseRTV state5(vec_init);
EXPECT(assert_equal(pt, state5.t(), tol));
EXPECT(assert_equal(rot, state5.R(), tol));
@ -83,7 +83,7 @@ TEST( testPoseRTV, Lie ) {
EXPECT(assert_equal(state1, state1.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
Vector delta = (Vec(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3);
Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3);
Rot3 rot2 = rot.retract(repeat(3, 0.1));
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) {
PoseRTV x0, x1, x2, x3, x4;
const double dt = 0.1;
Vector accel = (Vec(3) << 0.2, 0.0, 0.0), gyro = (Vec(3) << 0.0, 0.0, 0.2);
Vector accel = (Vector(3) << 0.2, 0.0, 0.0), gyro = (Vector(3) << 0.0, 0.0, 0.2);
Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6);
x1 = x0.generalDynamics(accel, gyro, dt);
@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) {
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMbn((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(PoseRTV::RRTMbn((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMnb((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(PoseRTV::RRTMnb((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
}
/* ************************************************************************* */

16
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -18,24 +18,24 @@ const double deg2rad = M_PI/180.0;
//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//LieVector v1((Vec(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w((Vec(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
LieVector V1_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP));
//LieVector v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0;
Vector gamma2 = (Vec(2) << 0.0, 0.0); // no shape
Vector u2 = (Vec(2) << 0.0, 0.0); // no control at time 2
Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape
Vector u2 = (Vector(2) << 0.0, 0.0); // no control at time 2
double distT = 1.0; // distance from the body-centered x axis to the big top motor
double distR = 5.0; // distance from the body-centered z axis to the small motor
Matrix Mass = diag((Vec(3) << mass, mass, mass));
Matrix Inertia = diag((Vec(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass));
Matrix Mass = diag((Vector(3) << mass, mass, mass));
Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass));
Vector computeFu(const Vector& gamma, const Vector& control) {
double gamma_r = gamma(0), gamma_p = gamma(1);
Matrix F = (Mat(6, 2) << distT*sin(gamma_r), 0.0,
Matrix F = (Matrix(6, 2) << distT*sin(gamma_r), 0.0,
distT*sin(gamma_p*cos(gamma_r)), 0.0,
0.0, distR,
sin(gamma_p)*cos(gamma_r), 0.0,
@ -100,7 +100,7 @@ TEST( Reconstruction, evaluateError) {
/* ************************************************************************* */
// Implement Newton-Euler equation for rigid body dynamics
Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
Matrix W = Pose3::adjointMap((Vec(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.));
Matrix W = Pose3::adjointMap((Vector(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.));
Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb);
return dV;
}

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
const double dt = 1.0;
PoseRTV origin,
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0)),
pose1a(Point3(0.5, 0.0, 0.0)),
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0));
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0));
/* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) {

16
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char** argv) {
// Create a prior on the first pose, placing it at the origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
@ -110,11 +110,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
@ -159,7 +159,7 @@ int main(int argc, char** argv) {
Key loopKey1(1000 * (0.0));
Key loopKey2(1000 * (5.0));
Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.25));
noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.25));
NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));
// This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
@ -189,11 +189,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation
@ -262,11 +262,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation

6
gtsam_unstable/examples/FixedLagSmootherExample.cpp
View File

@ -78,7 +78,7 @@ int main(int argc, char** argv) {
// Create a prior on the first pose, placing it at the origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
@ -104,11 +104,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05));
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Update the smoothers with the new factors

2
gtsam_unstable/geometry/BearingS2.cpp
View File

@ -29,7 +29,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) {
Matrix Cnb = A.rotation().matrix().transpose();
// Cbc = [0,0,1;0,1,0;-1,0,0];
Matrix Cbc = (Mat(3,3) <<
Matrix Cbc = (Matrix(3,3) <<
0.,0.,1.,
0.,1.,0.,
-1.,0.,0.);

4
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -134,7 +134,7 @@ public:
double H34 = 0;
double H35 = cos_phi/rho;
*H2 = J2 * (Mat(3, 5) <<
*H2 = J2 * (Matrix(3, 5) <<
H11, H12, H13, H14, H15,
H21, H22, H23, H24, H25,
H31, H32, H33, H34, H35);
@ -143,7 +143,7 @@ public:
double H16 = -cos_phi*cos_theta/rho2;
double H26 = -cos_phi*sin_theta/rho2;
double H36 = -sin_phi/rho2;
*H3 = J2 * (Mat(3, 1) <<
*H3 = J2 * (Matrix(3, 1) <<
H16,
H26,
H36);

4
gtsam_unstable/geometry/triangulation.h
View File

@ -60,7 +60,7 @@ public:
* @param rank_tol SVD rank tolerance
* @return Triangulated Point3
*/
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices, const std::vector<Point2>& measurements, double rank_tol);
GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices, const std::vector<Point2>& measurements, double rank_tol);
// Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem
// We should have a projectionfactor that knows pose is fixed
@ -135,7 +135,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @param landmarkKey to refer to landmark
* @return refined Point3
*/
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey);
GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey);
/**
* Given an initial estimate , refine a point using measurements in several cameras

4
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -60,8 +60,8 @@ TEST( BatchFixedLagSmoother, Example )
// SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true);
// Set up parameters
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
// Create a Fixed-Lag Smoother
typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps;

8
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -39,13 +39,13 @@ namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

8
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -39,13 +39,13 @@ namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

10
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -38,14 +38,14 @@ namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
//const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) );
const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
//const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) );
// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

8
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -39,13 +39,13 @@ namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

8
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -38,13 +38,13 @@ namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

4
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -59,8 +59,8 @@ TEST( IncrementalFixedLagSmoother, Example )
SETDEBUG("IncrementalFixedLagSmoother update", true);
// Set up parameters
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
// Create a Fixed-Lag Smoother
typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;

10
gtsam_unstable/slam/AHRS.cpp
View File

@ -60,7 +60,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
sigmas_v_a_ = esqrt(T * Pa_.diagonal());
// gravity in nav frame
n_g_ = (Vec(3) << 0.0, 0.0, g_e);
n_g_ = (Vector(3) << 0.0, 0.0, g_e);
n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!!
}
@ -72,7 +72,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
double sp = sin(mech0_.bRn().inverse().pitch());
double cy = cos(0.0);
double sy = sin(0.0);
Matrix Omega_T = (Mat(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0);
Matrix Omega_T = (Matrix(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0);
// Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb
Vector b_g = mech0_.b_g(g_e);
@ -82,7 +82,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
double g23 = g2 * g2 + g3 * g3;
double g123 = g1 * g1 + g23;
double f = 1 / (std::sqrt(g23) * g123);
Matrix H_g = (Mat(3, 3) <<
Matrix H_g = (Matrix(3, 3) <<
0.0, g3 / g23, -(g2 / g23), // roll
std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch
0.0, 0.0, 0.0); // we don't know anything on yaw
@ -220,8 +220,8 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
Matrix b_g = skewSymmetric(increment* f_previous);
Matrix H = collect(3, &b_g, &I3, &Z3);
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
// Matrix R = diag((Vec(3) << 1.0, 0.2, 1.0)); // good for L_twice
Matrix R = diag((Vec(3) << 0.01, 0.0001, 0.01));
// Matrix R = diag((Vector(3) << 1.0, 0.2, 1.0)); // good for L_twice
Matrix R = diag((Vector(3) << 0.01, 0.0001, 0.01));
// update the Kalman filter
KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -557,12 +557,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = (Mat(3, 3) <<
Matrix ENU_to_NED = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = (Mat(3, 3) <<
Matrix NED_to_ENU = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
@ -613,7 +613,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF((Vec(3) << 0.0, 0.0, 7.292115e-5));
Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g
@ -627,7 +627,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = (Vec(3) << 0.0, 0.0, -g_calc);
g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
// Calculate rho
@ -636,7 +636,7 @@ public:
double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = (Vec(3) << rho_E, rho_N, rho_U);
rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
}
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

4
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -481,12 +481,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = (Mat(3, 3) <<
Matrix ENU_to_NED = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = (Mat(3, 3) <<
Matrix NED_to_ENU = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);

10
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -290,12 +290,12 @@ public:
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
Matrix ENU_to_NED = (Mat(3, 3) <<
Matrix ENU_to_NED = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
Matrix NED_to_ENU = (Mat(3, 3) <<
Matrix NED_to_ENU = (Matrix(3, 3) <<
0.0, 1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, -1.0);
@ -346,7 +346,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF((Vec(3) << 0.0, 0.0, 7.292115e-5));
Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g
@ -360,7 +360,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = (Vec(3) << 0.0, 0.0, -g_calc);
g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
// Calculate rho
@ -369,7 +369,7 @@ public:
double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = (Vec(3) << rho_E, rho_N, rho_U);
rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
}
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

4
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -99,7 +99,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return (Vec(1) << 0.0);
return (Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives
@ -218,7 +218,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return (Vec(1) << 0.0);
return (Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives

8
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -13,8 +13,8 @@ namespace gtsam {
/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
const std::list<Vector>& F, const double g_e, bool flat) {
Matrix Umat = (Mat(3, U.size()) << concatVectors(U));
Matrix Fmat = (Mat(3, F.size()) << concatVectors(F));
Matrix Umat = (Matrix(3, U.size()) << concatVectors(U));
Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F));
return initialize(Umat, Fmat, g_e, flat);
}
@ -33,14 +33,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
if(g_e == 0) {
if (flat)
// acceleration measured is along the z-axis.
b_g = (Vec(3) << 0.0, 0.0, norm_2(meanF));
b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF));
else
// acceleration measured is the opposite of gravity (10.13)
b_g = -meanF;
} else {
if (flat)
// gravity is downward along the z-axis since we are flat on the ground
b_g = (Vec(3) << 0.0,0.0,g_e);
b_g = (Vector(3) << 0.0,0.0,g_e);
else
// normalize b_g and attribute remainder to biases
b_g = - g_e * meanF/meanF.norm();

12
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -16,22 +16,22 @@ using namespace std;
using namespace gtsam;
// stationary interval of gyro U and acc F
Matrix stationaryU = trans((Mat(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
Matrix stationaryF = trans((Mat(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
Matrix stationaryU = trans((Matrix(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
Matrix stationaryF = trans((Matrix(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
double g_e = 9.7963; // Atlanta
/* ************************************************************************* */
TEST (AHRS, cov) {
// samples stored by row
Matrix A = (Mat(4, 3) <<
Matrix A = (Matrix(4, 3) <<
1.0, 2.0, 3.0,
5.0, 7.0, 0.0,
9.0, 4.0, 7.0,
6.0, 3.0, 2.0);
Matrix actual = cov(trans(A));
Matrix expected = (Mat(3, 3) <<
Matrix expected = (Matrix(3, 3) <<
10.9167, 2.3333, 5.0000,
2.3333, 4.6667, -2.6667,
5.0000, -2.6667, 8.6667);
@ -43,7 +43,7 @@ TEST (AHRS, cov) {
TEST (AHRS, covU) {
Matrix actual = cov(10000*stationaryU);
Matrix expected = (Mat(3, 3) <<
Matrix expected = (Matrix(3, 3) <<
33.3333333, -1.66666667, 53.3333333,
-1.66666667, 0.333333333, -5.16666667,
53.3333333, -5.16666667, 110.333333);
@ -55,7 +55,7 @@ TEST (AHRS, covU) {
TEST (AHRS, covF) {
Matrix actual = cov(100*stationaryF);
Matrix expected = (Mat(3, 3) <<
Matrix expected = (Matrix(3, 3) <<
63.3808333, -0.432166667, -48.1706667,
-0.432166667, 8.31053333, -16.6792667,
-48.1706667, -16.6792667, 71.4297333);

Some files were not shown because too many files have changed in this diff Show More