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

Updated all comma initializer usages to use .finished()

release/4.3a0
Richard Roberts 2014-11-22 16:35:27 -08:00
parent fe43ea7116
commit aa093a35da
192 changed files with 1381 additions and 1383 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
.cproject
View File

@ -1,19 +1,17 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -21,7 +19,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.451252615" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths"/>
@ -62,13 +60,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -118,13 +116,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">

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((Vector(2) << 0.5, 0.5));
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5).finished());
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) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vector(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).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
// 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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(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((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(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((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished());
graph.add(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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(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((Vector(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).finished()); // 30cm std on x,y, 0.1 rad on theta
graph.add(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((Vector(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).finished()); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(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((Vector(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).finished()); // 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((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished());
graph.add(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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses

2
examples/Pose2SLAMExample_g2o.cpp
View File

@ -64,7 +64,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished());
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;

4
examples/Pose2SLAMExample_graph.cpp
View File

@ -31,14 +31,14 @@ 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((Vector(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).finished());
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, 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((Vector(3) << 0.01, 0.01, 0.01));
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01).finished());
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((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished());
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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished());
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();

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((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished());
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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
// 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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished());
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print

2
examples/Pose3SLAMExample_g2o.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;

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((Vector(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)).finished()); // 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

2
examples/SFMExample_SmartFactor.cpp
View File

@ -108,7 +108,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 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)));
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is

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((Vector(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)).finished()); // 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((Vector(5) << 500, 500, 0.1, 100, 100));
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
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((Vector(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)).finished()); // 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

@ -108,7 +108,7 @@ int main(int argc, char* argv[]) {
if (i == 0) {
// Add a prior on pose x0, with 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)));
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(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((Vector(2) << 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished());
// 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 = (Vector(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
Vector u = (Vector(2) << 1.0, 0.0).finished();
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished(), 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((Vector(2) << 0.25, 0.25), true);
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25).finished(), 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 (Vector(1) << (t2.value() - value())); }
Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); }
// 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 (Vector(1) << p.value()); }
static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {

2
gtsam/base/LieVector.h
View File

@ -42,7 +42,7 @@ struct LieVector : public Vector {
#endif
/** wrap a double */
LieVector(double d) : Vector((Vector(1) << d)) {}
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);

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

@ -34,7 +34,7 @@ TEST(cholesky, choleskyPartial) {
0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347,
0., 0., 0., 0., 3.0788, 2.6283, 2.3791,
0., 0., 0., 0., 0., 2.9227, 2.4056,
0., 0., 0., 0., 0., 0., 2.5776);
0., 0., 0., 0., 0., 0., 2.5776).finished();
// Do partial Cholesky on 3 frontal scalar variables
Matrix RSL(ABC);
@ -57,7 +57,7 @@ TEST(cholesky, choleskyPartial) {
TEST(cholesky, BadScalingCholesky) {
Matrix A = (Matrix(2,2) <<
1e-40, 0.0,
0.0, 1.0);
0.0, 1.0).finished();
Matrix R(A.transpose() * A);
choleskyPartial(R, 2);
@ -72,7 +72,7 @@ TEST(cholesky, BadScalingCholesky) {
TEST(cholesky, BadScalingSVD) {
Matrix A = (Matrix(2,2) <<
1.0, 0.0,
0.0, 1e-40);
0.0, 1e-40).finished();
Matrix U, V;
Vector S;

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

@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix)
/* ************************************************************************* */
TEST( LieMatrix, construction ) {
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0);
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished();
LieMatrix lie1(m), lie2(m);
EXPECT(lie1.dim() == 4);
@ -37,7 +37,7 @@ TEST( LieMatrix, construction ) {
/* ************************************************************************* */
TEST( LieMatrix, other_constructors ) {
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished();
LieMatrix exp(init);
double data[] = {10,30,20,40};
LieMatrix b(2,2,data);
@ -46,10 +46,10 @@ TEST( LieMatrix, other_constructors ) {
/* ************************************************************************* */
TEST(LieMatrix, retract) {
LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0));
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0);
LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished());
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished();
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0));
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished());
LieMatrix actual = init.retract(update);
EXPECT(assert_equal(expected, actual));
@ -59,8 +59,8 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4);
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0)));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished();
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}

2
gtsam/base/tests/testLieScalar.cpp
View File

@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2)));
EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2)));
}
/* ************************************************************************* */

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

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

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

@ -32,7 +32,7 @@ static const double tol = 1e-9;
/* ************************************************************************* */
TEST( matrix, constructor_data )
{
Matrix A = (Matrix(2, 2) << -5, 3, 0, -5);
Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished();
Matrix B(2, 2);
B(0, 0) = -5;
@ -46,7 +46,7 @@ TEST( matrix, constructor_data )
/* ************************************************************************* */
TEST( matrix, Matrix_ )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0);
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished();
Matrix B(2, 2);
B(0, 0) = -5;
B(0, 1) = 3;
@ -82,17 +82,17 @@ TEST( matrix, special_comma_initializer)
expected(1,0) = 3;
expected(1,1) = 4;
Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4);
Matrix actual2((Matrix(2,2) << 1, 2, 3, 4));
Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4).finished();
Matrix actual2((Matrix(2,2) << 1, 2, 3, 4).finished());
Matrix submat1 = (Matrix(1,2) << 3, 4);
Matrix actual3 = (Matrix(2,2) << 1, 2, submat1);
Matrix submat1 = (Matrix(1,2) << 3, 4).finished();
Matrix actual3 = (Matrix(2,2) << 1, 2, submat1).finished();
Matrix submat2 = (Matrix(1,2) << 1, 2);
Matrix actual4 = (Matrix(2,2) << submat2, 3, 4);
Matrix submat2 = (Matrix(1,2) << 1, 2).finished();
Matrix actual4 = (Matrix(2,2) << submat2, 3, 4).finished();
Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4));
Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4));
Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4).finished());
Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4).finished());
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
@ -105,7 +105,7 @@ TEST( matrix, special_comma_initializer)
/* ************************************************************************* */
TEST( matrix, col_major )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double * const a = &A(0, 0);
EXPECT_DOUBLES_EQUAL(1, a[0], tol);
EXPECT_DOUBLES_EQUAL(3, a[1], tol);
@ -116,8 +116,8 @@ TEST( matrix, col_major )
/* ************************************************************************* */
TEST( matrix, collect1 )
{
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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
Matrix AB = collect(2, &A, &B);
Matrix C(2, 5);
for (int i = 0; i < 2; i++)
@ -133,8 +133,8 @@ TEST( matrix, collect1 )
/* ************************************************************************* */
TEST( matrix, collect2 )
{
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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
vector<const Matrix*> matrices;
matrices.push_back(&A);
matrices.push_back(&B);
@ -162,7 +162,7 @@ TEST( matrix, collect3 )
Matrix AB = collect(matrices, 2, 3);
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);
0.0, 1.0, 0.0, 0.0, 1.0, 0.0).finished();
EQUALITY(exp,AB);
}
@ -170,8 +170,8 @@ TEST( matrix, collect3 )
/* ************************************************************************* */
TEST( matrix, stack )
{
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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
Matrix AB = stack(2, &A, &B);
Matrix C(5, 2);
for (int i = 0; i < 2; i++)
@ -195,17 +195,17 @@ TEST( matrix, column )
{
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);
-0.1).finished();
Vector a1 = column(A, 0);
Vector exp1 = (Vector(4) << -1., 0., 1., 0.);
Vector exp1 = (Vector(4) << -1., 0., 1., 0.).finished();
EXPECT(assert_equal(a1, exp1));
Vector a2 = column(A, 3);
Vector exp2 = (Vector(4) << 0., 1., 0., 0.);
Vector exp2 = (Vector(4) << 0., 1., 0., 0.).finished();
EXPECT(assert_equal(a2, exp2));
Vector a3 = column(A, 6);
Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1);
Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished();
EXPECT(assert_equal(a3, exp3));
}
@ -223,7 +223,7 @@ TEST( matrix, insert_column )
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,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
@ -246,7 +246,7 @@ TEST( matrix, insert_subcolumn )
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, 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).finished();
EXPECT(assert_equal(expected, big));
}
@ -256,17 +256,17 @@ TEST( matrix, row )
{
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);
-0.1).finished();
Vector a1 = row(A, 0);
Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2);
Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2).finished();
EXPECT(assert_equal(a1, exp1));
Vector a2 = row(A, 2);
Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2);
Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2).finished();
EXPECT(assert_equal(a2, exp2));
Vector a3 = row(A, 3);
Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1);
Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1).finished();
EXPECT(assert_equal(a3, exp3));
}
@ -290,13 +290,13 @@ TEST( matrix, zeros )
TEST( matrix, insert_sub )
{
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0);
1.0).finished();
insertSub(big, small, 1, 2);
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);
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
@ -320,7 +320,7 @@ TEST( matrix, diagMatrices )
0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0);
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0).finished();
EXPECT(assert_equal(expected, actual));
}
@ -330,7 +330,7 @@ TEST( matrix, stream_read ) {
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);
1.2, 3.4, 4.5, 6.7).finished();
string matrixAsString =
"1.1 2.3 4.2 7.6\n"
@ -362,7 +362,7 @@ TEST( matrix, scale_columns )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vector(4) << 2., 3., 4., 5.);
Vector v = (Vector(4) << 2., 3., 4., 5.).finished();
Matrix actual = vector_scale(A, v);
@ -400,7 +400,7 @@ TEST( matrix, scale_rows )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vector(3) << 2., 3., 4.);
Vector v = (Vector(3) << 2., 3., 4.).finished();
Matrix actual = vector_scale(v, A);
@ -438,7 +438,7 @@ TEST( matrix, scale_rows_mask )
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = (Vector(3) << 2., std::numeric_limits<double>::infinity(), 4.);
Vector v = (Vector(3) << 2., std::numeric_limits<double>::infinity(), 4.).finished();
Matrix actual = vector_scale(v, A, true);
@ -537,18 +537,18 @@ TEST( matrix, equal_nan )
/* ************************************************************************* */
TEST( matrix, addition )
{
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);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished();
EQUALITY(A+B,C);
}
/* ************************************************************************* */
TEST( matrix, addition_in_place )
{
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);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished();
A += B;
EQUALITY(A,C);
}
@ -556,18 +556,18 @@ TEST( matrix, addition_in_place )
/* ************************************************************************* */
TEST( matrix, subtraction )
{
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);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
EQUALITY(A-B,C);
}
/* ************************************************************************* */
TEST( matrix, subtraction_in_place )
{
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);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
A -= B;
EQUALITY(A,C);
}
@ -617,10 +617,10 @@ TEST( matrix, matrix_vector_multiplication )
{
Vector result(2);
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.);
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Vector v = (Vector(3) << 1., 2., 3.).finished();
Vector Av = (Vector(2) << 14., 32.).finished();
Vector AtAv = (Vector(3) << 142., 188., 234.).finished();
EQUALITY(A*v,Av);
EQUALITY(A^Av,AtAv);
@ -657,12 +657,12 @@ TEST( matrix, zero_below_diagonal ) {
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);
1.0, 2.0, 3.0, 4.0).finished();
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);
0.0, 0.0, 3.0, 4.0).finished();
Matrix actual1r = A1;
zeroBelowDiagonal(actual1r);
EXPECT(assert_equal(expected1, actual1r, 1e-10));
@ -680,13 +680,13 @@ TEST( matrix, zero_below_diagonal ) {
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).finished();
Matrix expected2 = (Matrix(5, 3) <<
1.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 0.0, 3.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0);
0.0, 0.0, 0.0).finished();
Matrix actual2r = A2;
zeroBelowDiagonal(actual2r);
@ -701,7 +701,7 @@ TEST( matrix, zero_below_diagonal ) {
0.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0);
0.0, 2.0, 3.0).finished();
actual2c = A2;
zeroBelowDiagonal(actual2c, 1);
EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10));
@ -739,17 +739,17 @@ TEST( matrix, inverse )
EXPECT(assert_equal(expected, Ainv, 1e-4));
// These two matrices failed before version 2003 because we called LU incorrectly
Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0));
Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished());
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
0.0, 0.0, 1.0).finished(),
inverse(lMg)));
Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0));
Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
0.0, 0.0, 1.0).finished(),
inverse(gMl)));
}
@ -787,19 +787,19 @@ TEST( matrix, inverse2 )
TEST( matrix, backsubtitution )
{
// TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = (Vector(2) << 3.6250, -0.75);
Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.);
Vector expected1 = (Vector(2) << 3.6250, -0.75).finished();
Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.).finished();
Vector b1 = U22 * expected1;
EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
// TEST TWO 3x3 matrix U2*x=b2
Vector expected2 = (Vector(3) << 5.5, -8.5, 5.);
Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.);
Vector expected2 = (Vector(3) << 5.5, -8.5, 5.).finished();
Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.).finished();
Vector b2 = U33 * expected2;
EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
// TEST THREE Lower triangular 3x3 matrix L3*x=b3
Vector expected3 = (Vector(3) << 1., 1., 1.);
Vector expected3 = (Vector(3) << 1., 1., 1.).finished();
Matrix L3 = trans(U33);
Vector b3 = L3 * expected3;
EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
@ -816,11 +816,11 @@ TEST( matrix, householder )
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);
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished();
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 );
00, 10,0, 0, 0, -10, -1 ).finished();
householder_(A1, 3);
EXPECT(assert_equal(expected1, A1, 1e-3));
@ -828,11 +828,11 @@ TEST( matrix, householder )
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);
0, 0, 4.4721, 0, -4.4721, 0.894).finished();
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);
00, 10,0, 0, 0, -10, -1).finished();
householder(A2, 3);
EXPECT(assert_equal(expected, A2, 1e-3));
}
@ -845,11 +845,11 @@ TEST( matrix, householder_colMajor )
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));
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished());
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));
00, 10,0, 0, 0, -10, -1).finished());
householder_(A1, 3);
EXPECT(assert_equal(expected1, A1, 1e-3));
@ -857,11 +857,11 @@ TEST( matrix, householder_colMajor )
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));
0, 0, 4.4721, 0, -4.4721, 0.894).finished());
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));
00, 10,0, 0, 0, -10, -1).finished());
householder(A2, 3);
EXPECT(assert_equal(expected, A2, 1e-3));
}
@ -875,11 +875,11 @@ TEST( matrix, eigen_QR )
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));
0, 0, 4.4721, 0, -4.4721, 0.894).finished());
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));
00, 10,0, 0, 0, -10, -1).finished());
Matrix actual = A.householderQr().matrixQR();
zeroBelowDiagonal(actual);
@ -889,7 +889,7 @@ TEST( matrix, eigen_QR )
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));
00, 10,0, 0, 0, -10, -1).finished());
inplace_QR(A);
EXPECT(assert_equal(expected, A, 1e-3));
}
@ -902,16 +902,16 @@ TEST( matrix, qr )
{
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);
0, 0, -10, 10, 0, -10, 0).finished();
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);
0, -0.5963, 0, 0, -0.4472).finished();
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);
7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
Matrix Q, R;
boost::tie(Q, R) = qr(A);
@ -924,10 +924,10 @@ TEST( matrix, qr )
TEST( matrix, sub )
{
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);
0, 00, 10, 0, 0, 0, -10).finished();
Matrix actual = sub(A, 1, 3, 1, 5);
Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10);
Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10).finished();
EQUALITY(actual,expected);
}
@ -935,15 +935,15 @@ TEST( matrix, sub )
/* ************************************************************************* */
TEST( matrix, trans )
{
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);
Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
EQUALITY(trans(A),B);
}
/* ************************************************************************* */
TEST( matrix, col_major_access )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0);
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double* a = &A(0, 0);
DOUBLES_EQUAL(2.0,a[2],1e-9);
}
@ -953,15 +953,15 @@ TEST( matrix, weighted_elimination )
{
// create a matrix to eliminate
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 = (Vector(4) << -0.2, 0.3, 0.2, -0.1);
Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1);
1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.).finished();
Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished();
Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished();
// expected values
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 = (Vector(4) << 0.2, -0.14, 0.0, 0.2);
Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
-0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.).finished();
Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished();
Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
Vector r;
double di, sigma;
@ -987,11 +987,11 @@ TEST( matrix, weighted_elimination )
TEST( matrix, inverse_square_root )
{
Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
0.0, 0.0, 0.0, 0.01);
0.0, 0.0, 0.0, 0.01).finished();
Matrix actual = inverse_square_root(measurement_covariance);
Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
10.0);
10.0).finished();
EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual));
@ -1007,14 +1007,14 @@ TEST( matrix, inverse_square_root )
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);
0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517).finished();
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,
0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937);
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937).finished();
EQUALITY(expected, inverse_square_root(M));
}
@ -1027,14 +1027,14 @@ 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);
-0.0021113, 0.0036415, 0.0909464).finished();
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,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
}
TEST( matrix, LLt )
{
@ -1053,8 +1053,8 @@ TEST( matrix, cholesky_inverse )
/* ************************************************************************* */
TEST( matrix, multiplyAdd )
{
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.),
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = (Vector(3) << 5., 6., 7.).finished(),
expected = e + A * x;
multiplyAdd(1, A, x, e);
@ -1064,8 +1064,8 @@ TEST( matrix, multiplyAdd )
/* ************************************************************************* */
TEST( matrix, transposeMultiplyAdd )
{
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.),
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = (Vector(3) << 5., 6., 7.).finished(),
expected = x + trans(A) * e;
transposeMultiplyAdd(1, A, e, x);
@ -1075,31 +1075,31 @@ TEST( matrix, transposeMultiplyAdd )
/* ************************************************************************* */
TEST( matrix, linear_dependent )
{
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);
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
TEST( matrix, linear_dependent2 )
{
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);
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
TEST( matrix, linear_dependent3 )
{
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);
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished();
EXPECT(linear_independent(A, B));
}
/* ************************************************************************* */
TEST( matrix, svd1 )
{
Vector v = (Vector(3) << 2., 1., 0.);
Vector v = (Vector(3) << 2., 1., 0.).finished();
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
* Matrix(trans(V1));
Matrix U, V;
@ -1112,7 +1112,7 @@ TEST( matrix, svd1 )
/* ************************************************************************* */
/// Sample A matrix for SVD
static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.);
static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished();
static Matrix sampleAt = trans(sampleA);
/* ************************************************************************* */
@ -1121,9 +1121,9 @@ TEST( matrix, svd2 )
Matrix U, V;
Vector s;
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.);
Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.).finished();
Vector expected_s = (Vector(2) << 3.,2.).finished();
Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.).finished();
svd(sampleA, U, s, V);
@ -1144,9 +1144,9 @@ TEST( matrix, svd3 )
Matrix U, V;
Vector s;
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.);
Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.).finished();
Vector expected_s = (Vector(2) << 3.0, 2.0).finished();
Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.).finished();
svd(sampleAt, U, s, V);
@ -1175,18 +1175,18 @@ TEST( matrix, svd4 )
Matrix A = (Matrix(3, 2) <<
0.8147, 0.9134,
0.9058, 0.6324,
0.1270, 0.0975);
0.1270, 0.0975).finished();
Matrix expectedU = (Matrix(3, 2) <<
0.7397, 0.6724,
0.6659, -0.7370,
0.0970, -0.0689);
0.0970, -0.0689).finished();
Vector expected_s = (Vector(2) << 1.6455, 0.1910);
Vector expected_s = (Vector(2) << 1.6455, 0.1910).finished();
Matrix expectedV = (Matrix(2, 2) <<
0.7403, -0.6723,
0.6723, 0.7403);
0.6723, 0.7403).finished();
svd(A, U, s, V);
@ -1220,12 +1220,12 @@ TEST( matrix, DLT )
1.56, 0.42, 4.56, -3.38, -0.91, -9.88, 22.36, 6.02, 65.36,
1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19,
2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64
);
).finished();
int rank;
double error;
Vector actual;
boost::tie(rank,error,actual) = DLT(A);
Vector expected = (Vector(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).finished();
EXPECT_LONGS_EQUAL(8,rank);
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
EXPECT(assert_equal(expected, actual, 1e-4));

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

@ -65,7 +65,7 @@ TEST(testNumericalDerivative, numericalHessian2) {
Vector2 x(v);
Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
* cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1)));
* cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))).finished();
Matrix actual = numericalHessian(f2, x);
@ -82,15 +82,15 @@ double f3(double x1, double x2) {
TEST(testNumericalDerivative, numericalHessian211) {
double x1 = 1, x2 = 5;
Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished();
Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2));
Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)).finished();
Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished();
Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
@ -104,27 +104,27 @@ double f4(double x, double y, double z) {
//
TEST(testNumericalDerivative, numericalHessian311) {
double x = 1, y = 2, z = 3;
Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished();
Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z);
Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z).finished();
Matrix actual12 = numericalHessian312<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z);
Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z).finished();
Matrix actual13 = numericalHessian313<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished();
Matrix actual22 = numericalHessian322<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z);
Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z).finished();
Matrix actual23 = numericalHessian323<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2);
Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2).finished();
Matrix actual33 = numericalHessian333<double, double, double>(f4, x, y, z);
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 = (Vector(2) << 1.0, 2.0);
Vector v2 = (Vector(2) << 3.0, 4.0);
Vector v3 = (Vector(2) << 5.0, 6.0);
Vector v1 = (Vector(2) << 1.0, 2.0).finished();
Vector v2 = (Vector(2) << 3.0, 4.0).finished();
Vector v3 = (Vector(2) << 5.0, 6.0).finished();
/* ************************************************************************* */
TEST (Serialization, FastList) {
@ -84,23 +84,23 @@ TEST (Serialization, FastVector) {
/* ************************************************************************* */
TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()));
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>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(equalityXML<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()));
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>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(equalityBinary<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector>((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()));
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>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
}
/* ************************************************************************* */

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

@ -31,7 +31,7 @@ static SymmetricBlockMatrix testBlockMatrix(
3, 9, 15, 16, 17, 18,
4, 10, 16, 22, 23, 24,
5, 11, 17, 23, 29, 30,
6, 12, 18, 24, 30, 36));
6, 12, 18, 24, 30, 36).finished());
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, ReadBlocks)
@ -39,7 +39,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
// On the diagonal
Matrix expected1 = (Matrix(2, 2) <<
22, 23,
23, 29);
23, 29).finished();
Matrix actual1 = testBlockMatrix(1, 1);
// Test only writing the upper triangle for efficiency
Matrix actual1t = Matrix::Zero(2, 2);
@ -51,14 +51,14 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
Matrix expected2 = (Matrix(3, 2) <<
4, 5,
10, 11,
16, 17);
16, 17).finished();
Matrix actual2 = testBlockMatrix(0, 1);
EXPECT(assert_equal(expected2, actual2));
// Below the diagonal
Matrix expected3 = (Matrix(2, 3) <<
4, 10, 16,
5, 11, 17);
5, 11, 17).finished();
Matrix actual3 = testBlockMatrix(1, 0);
EXPECT(assert_equal(expected3, actual3));
}
@ -102,7 +102,7 @@ TEST(SymmetricBlockMatrix, Ranges)
Matrix expected1 = (Matrix(3, 3) <<
22, 23, 24,
23, 29, 30,
24, 30, 36);
24, 30, 36).finished();
Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView();
Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3);
EXPECT(assert_equal(expected1, actual1));
@ -112,7 +112,7 @@ TEST(SymmetricBlockMatrix, Ranges)
Matrix expected2 = (Matrix(3, 1) <<
24,
30,
36);
36).finished();
Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal();
Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3);
EXPECT(assert_equal(expected2, actual2));
@ -122,7 +122,7 @@ TEST(SymmetricBlockMatrix, Ranges)
Matrix expected3 = (Matrix(3, 3) <<
4, 10, 16,
5, 11, 17,
6, 12, 18);
6, 12, 18).finished();
Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal();
Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1);
EXPECT(assert_equal(expected3, actual3));
@ -138,7 +138,7 @@ TEST(SymmetricBlockMatrix, expressions)
0, 0, 4, 6, 8, 0,
0, 0, 0, 9, 12, 0,
0, 0, 0, 0, 16, 0,
0, 0, 0, 0, 0, 0));
0, 0, 0, 0, 0, 0).finished());
SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) <<
0, 0, 10, 15, 20, 0,
@ -146,10 +146,10 @@ TEST(SymmetricBlockMatrix, expressions)
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, 0, 0, 0).finished());
Matrix a = (Matrix(1, 3) << 2, 3, 4);
Matrix b = (Matrix(1, 2) << 5, 6);
Matrix a = (Matrix(1, 3) << 2, 3, 4).finished();
Matrix b = (Matrix(1, 2) << 5, 6).finished();
SymmetricBlockMatrix bm1(list_of(2)(3)(1));
bm1.full().triangularView().setZero();

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

@ -47,17 +47,17 @@ TEST( TestVector, special_comma_initializer)
expected(1) = 2;
expected(2) = 3;
Vector actual1 = (Vector(3) << 1, 2, 3);
Vector actual2((Vector(3) << 1, 2, 3));
Vector actual1 = (Vector(3) << 1, 2, 3).finished();
Vector actual2((Vector(3) << 1, 2, 3).finished());
Vector subvec1 = (Vector(2) << 2, 3);
Vector actual4 = (Vector(3) << 1, subvec1);
Vector subvec1 = (Vector(2) << 2, 3).finished();
Vector actual4 = (Vector(3) << 1, subvec1).finished();
Vector subvec2 = (Vector(2) << 1, 2);
Vector actual5 = (Vector(3) << subvec2, 3);
Vector subvec2 = (Vector(2) << 1, 2).finished();
Vector actual5 = (Vector(3) << subvec2, 3).finished();
Vector actual6 = testFcn1((Vector(3) << 1, 2, 3));
Vector actual7 = testFcn2((Vector(3) << 1, 2, 3));
Vector actual6 = testFcn1((Vector(3) << 1, 2, 3).finished());
Vector actual7 = testFcn2((Vector(3) << 1, 2, 3).finished());
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
@ -142,7 +142,7 @@ TEST( TestVector, subInsert )
size_t i = 2;
subInsert(big, small, i);
Vector expected = (Vector(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).finished();
EXPECT(assert_equal(expected, big));
}
@ -240,13 +240,13 @@ TEST( TestVector, weightedPseudoinverse_constraint )
/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_nan )
{
Vector a = (Vector(4) << 1., 0., 0., 0.);
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.);
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
Vector expected = (Vector(4) << 1., 0., 0.,0.);
Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
EXPECT(assert_equal(expected, pseudo));
DOUBLES_EQUAL(100, precision, 1e-5);
}
@ -254,31 +254,31 @@ TEST( TestVector, weightedPseudoinverse_nan )
/* ************************************************************************* */
TEST( TestVector, ediv )
{
Vector a = (Vector(3) << 10., 20., 30.);
Vector b = (Vector(3) << 2.0, 5.0, 6.0);
Vector a = (Vector(3) << 10., 20., 30.).finished();
Vector b = (Vector(3) << 2.0, 5.0, 6.0).finished();
Vector actual(ediv(a,b));
Vector c = (Vector(3) << 5.0, 4.0, 5.0);
Vector c = (Vector(3) << 5.0, 4.0, 5.0).finished();
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
TEST( TestVector, dot )
{
Vector a = (Vector(3) << 10., 20., 30.);
Vector b = (Vector(3) << 2.0, 5.0, 6.0);
Vector a = (Vector(3) << 10., 20., 30.).finished();
Vector b = (Vector(3) << 2.0, 5.0, 6.0).finished();
DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9);
}
/* ************************************************************************* */
TEST( TestVector, axpy )
{
Vector x = (Vector(3) << 10., 20., 30.);
Vector y0 = (Vector(3) << 2.0, 5.0, 6.0);
Vector x = (Vector(3) << 10., 20., 30.).finished();
Vector y0 = (Vector(3) << 2.0, 5.0, 6.0).finished();
Vector y1 = y0, y2 = y0;
axpy(0.1,x,y1);
axpy(0.1,x,y2.head(3));
Vector expected = (Vector(3) << 3.0, 7.0, 9.0);
Vector expected = (Vector(3) << 3.0, 7.0, 9.0).finished();
EXPECT(assert_equal(expected,y1));
EXPECT(assert_equal(expected,Vector(y2)));
}
@ -286,8 +286,8 @@ TEST( TestVector, axpy )
/* ************************************************************************* */
TEST( TestVector, equals )
{
Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
Vector v2 = (Vector(1) << 1.0);
Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan
Vector v2 = (Vector(1) << 1.0).finished();
double tol = 1.;
EXPECT(!equal_with_abs_tol(v1, v2, tol));
}
@ -295,7 +295,7 @@ TEST( TestVector, equals )
/* ************************************************************************* */
TEST( TestVector, greater_than )
{
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0),
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0).finished(),
v2 = zero(3);
EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
@ -304,31 +304,31 @@ TEST( TestVector, greater_than )
/* ************************************************************************* */
TEST( TestVector, reciprocal )
{
Vector v = (Vector(3) << 1.0, 2.0, 4.0);
EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25),reciprocal(v)));
Vector v = (Vector(3) << 1.0, 2.0, 4.0).finished();
EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25).finished(),reciprocal(v)));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent )
{
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0);
Vector v2 = (Vector(3) << -2.0, -4.0, -6.0);
Vector v1 = (Vector(3) << 1.0, 2.0, 3.0).finished();
Vector v2 = (Vector(3) << -2.0, -4.0, -6.0).finished();
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent2 )
{
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vector(3) << 0.0, -4.0, 0.0);
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0).finished();
Vector v2 = (Vector(3) << 0.0, -4.0, 0.0).finished();
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent3 )
{
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0);
Vector v2 = (Vector(3) << 0.1, -4.1, 0.0);
Vector v1 = (Vector(3) << 0.0, 2.0, 0.0).finished();
Vector v2 = (Vector(3) << 0.1, -4.1, 0.0).finished();
EXPECT(!linear_dependent(v1, v2));
}

2
gtsam/base/tests/testVerticalBlockMatrix.cpp
View File

@ -43,7 +43,7 @@ TEST(VerticalBlockMatrix, Constructor2) {
3, 9, 15, 16, 17, 18, //
4, 10, 16, 22, 23, 24, //
5, 11, 17, 23, 29, 30, //
6, 12, 18, 24, 30, 36));
6, 12, 18, 24, 30, 36).finished());
EXPECT_LONGS_EQUAL(6,actual.rows());
EXPECT_LONGS_EQUAL(6,actual.cols());
EXPECT_LONGS_EQUAL(3,actual.nBlocks());

4
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((Vector(2) << 0.359631, 0.640369), actualCvector, 1e-6));
EXPECT(assert_equal((Vector(2) << 0.359631, 0.640369).finished(), actualCvector, 1e-6));
actualCvector = marginals.marginalProbabilities(Mark);
EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372), actualCvector, 1e-6));
EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372).finished(), actualCvector, 1e-6));
}
/* ************************************************************************* */

6
gtsam/geometry/Cal3Bundler.cpp
View File

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

4
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -29,12 +29,12 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(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).finished();
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished();
}
/* ************************************************************************* */

4
gtsam/geometry/Cal3Unified.cpp
View File

@ -30,13 +30,13 @@ Cal3Unified::Cal3Unified(const Vector &v):
/* ************************************************************************* */
Vector Cal3Unified::vector() const {
return (Vector(10) << Base::vector(), xi_);
return (Vector(10) << Base::vector(), xi_).finished();
}
/* ************************************************************************* */
void Cal3Unified::print(const std::string& s) const {
Base::print(s);
gtsam::print((Vector)(Vector(1) << xi_), s + ".xi");
gtsam::print((Vector)(Vector(1) << xi_).finished(), s + ".xi");
}
/* ************************************************************************* */

4
gtsam/geometry/Cal3_S2.h
View File

@ -126,7 +126,7 @@ public:
/// return calibration matrix K
Matrix K() const {
return (Matrix(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).finished();
}
/** @deprecated The following function has been deprecated, use K above */
@ -138,7 +138,7 @@ public:
Matrix matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
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);
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
}
/**

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 = (Matrix(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).finished();
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@ -85,13 +85,13 @@ Point2 CalibratedCamera::project(const Point3& point,
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v);
-uv, -u, 0., -d, d * v).finished();
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
*Dpoint = d
* (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));
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished();
}
#endif
}

4
gtsam/geometry/EssentialMatrix.cpp
View File

@ -55,8 +55,8 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
/* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
return Vector(5) << //
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
return (Vector(5) <<
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished();
}
/* ************************************************************************* */

2
gtsam/geometry/EssentialMatrix.h
View File

@ -32,7 +32,7 @@ public:
/// Static function to convert Point2 to homogeneous coordinates
static Vector Homogeneous(const Point2& p) {
return Vector(3) << p.x(), p.y(), 1;
return (Vector(3) << p.x(), p.y(), 1).finished();
}
/// @name Constructors and named constructors

2
gtsam/geometry/Point2.h
View File

@ -171,7 +171,7 @@ public:
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
/// @}
/// @name Vector Space

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 = (Matrix(3, 1) << t_.x(), t_.y(), 1.0);
Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished();
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 (Vector(3) << t.x(), t.y(), w);
return (Vector(3) << t.x(), t.y(), w).finished();
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 (Vector(3) << v.x(), v.y(), w);
return (Vector(3) << v.x(), v.y(), w).finished();
}
}
@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
return Logmap(between(p2));
#else
Pose2 r = between(p2);
return (Vector(3) << r.x(), r.y(), r.theta());
return (Vector(3) << r.x(), r.y(), r.theta()).finished();
#endif
}
@ -114,7 +114,7 @@ Matrix Pose2::AdjointMap() const {
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
);
).finished();
}
/* ************************************************************************* */
@ -153,7 +153,7 @@ Point2 Pose2::transform_to(const Point2& point,
if (!H1 && !H2) return q;
if (H1) *H1 = (Matrix(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
0.0, -1.0, -q.x()).finished();
if (H2) *H2 = r_.transpose();
return q;
}
@ -175,7 +175,7 @@ Point2 Pose2::transform_from(const Point2& p,
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x());
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
@ -265,7 +265,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
*H1 = (Matrix(3, 3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0);
0.0, 0.0,-1.0).finished();
}
if (H2) *H2 = I3;
@ -305,7 +305,7 @@ double Pose2::range(const Point2& point,
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
-r_.s(), -r_.c(), 0.0).finished();
if (H2) *H2 = H;
return r;
}
@ -320,10 +320,10 @@ double Pose2::range(const Pose2& pose2,
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
-r_.s(), -r_.c(), 0.0).finished();
if (H2) *H2 = H * (Matrix(2, 3) <<
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0);
pose2.r_.s(), pose2.r_.c(), 0.0).finished();
return r;
}

2
gtsam/geometry/Pose2.h
View File

@ -183,7 +183,7 @@ public:
return (Matrix(3,3) <<
0.,-w, vx,
w, 0., vy,
0., 0., 0.);
0., 0., 0.).finished();
}
/// @}

4
gtsam/geometry/Pose3.cpp
View File

@ -99,7 +99,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation
Matrix res = I6;
Matrix6 ad_i = I6;
@ -333,7 +333,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 = (Matrix(1, 3) << x / n, y / n, z / n);
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished();
if (H1)
*H1 = D_result_d * (*H1);
if (H2)

4
gtsam/geometry/Pose3.h
View File

@ -203,7 +203,7 @@ public:
* as detailed in [Kobilarov09siggraph] eq. (15)
* The full formula is documented in [Celledoni99cmame]
* Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and
* time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421<EFBFBD> 438, 2003.
* time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003.
* and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2
* Ernst Hairer, et al., Geometric Numerical Integration,
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
@ -222,7 +222,7 @@ public:
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.);
0., 0., 0., 0.).finished();
}
/// @}

12
gtsam/geometry/Rot2.cpp
View File

@ -65,12 +65,12 @@ Rot2& Rot2::normalize() {
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return (Matrix(2, 2) << c_, -s_, s_, c_);
return (Matrix(2, 2) << c_, -s_, s_, c_).finished();
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return (Matrix(2, 2) << c_, s_, -s_, c_);
return (Matrix(2, 2) << c_, s_, -s_, c_).finished();
}
/* ************************************************************************* */
@ -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 = (Matrix(2, 1) << -q.y(), q.x());
if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
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 = (Matrix(2, 1) << q.y(), -q.x()); // R_{pi/2}q
if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // 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 = (Matrix(1, 2) << -y / d2, x / d2);
if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished();
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = (Matrix(1, 2) << 0.0, 0.0);
if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished();
return Rot2();
}
}

2
gtsam/geometry/Rot2.h
View File

@ -170,7 +170,7 @@ namespace gtsam {
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) {
return (Vector(1) << r.theta());
return (Vector(1) << r.theta()).finished();
}
/// @}

2
gtsam/geometry/Rot3.h
View File

@ -193,7 +193,7 @@ namespace gtsam {
* @return incremental rotation matrix
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez((Vector(3) << wx, wy, wz));}
{ return rodriguez((Vector(3) << wx, wy, wz).finished());}
/// @}
/// @name Testable

6
gtsam/geometry/StereoCamera.cpp
View File

@ -68,7 +68,7 @@ namespace gtsam {
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
);
).finished();
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
@ -76,7 +76,7 @@ namespace gtsam {
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
);
).finished();
}
#endif
}
@ -94,7 +94,7 @@ namespace gtsam {
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()
);
).finished();
}
}

4
gtsam/geometry/Unit3.cpp
View File

@ -162,9 +162,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
// Check for special cases
if (std::abs(dot - 1.0) < 1e-16)
return (Vector(2) << 0, 0);
return (Vector(2) << 0, 0).finished();
else if (std::abs(dot + 1.0) < 1e-16)
return (Vector(2) << M_PI, 0);
return (Vector(2) << M_PI, 0).finished();
else {
// no special case
double theta = acos(dot);

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

@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate)
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished();
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p);

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

@ -31,7 +31,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
).finished(),
Point3(0,0,0.5));
static const CalibratedCamera camera(pose1);

10
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -62,16 +62,16 @@ TEST (EssentialMatrix, retract0) {
//*************************************************************************
TEST (EssentialMatrix, retract1) {
EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), Unit3(c1Tc2));
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0));
EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0).finished()), Unit3(c1Tc2));
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished());
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, retract2) {
EssentialMatrix expected(c1Rc2,
Unit3(c1Tc2).retract((Vector(2) << 0.1, 0)));
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0));
Unit3(c1Tc2).retract((Vector(2) << 0.1, 0).finished()));
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished());
EXPECT(assert_equal(expected, actual));
}
@ -85,7 +85,7 @@ TEST (EssentialMatrix, transform_to) {
* Rot3::roll(M_PI / 6.0);
Point3 aTb2(19.2, 3.7, 5.9);
EssentialMatrix E(aRb2, Unit3(aTb2));
//EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0)));
//EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0).finished()));
static Point3 P(0.2, 0.7, -2);
Matrix actH1, actH2;
E.transform_to(P, actH1, actH2);

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

@ -33,7 +33,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
).finished(),
Point3(0,0,0.5));
typedef PinholeCamera<Cal3_S2> Camera;

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((Vector(2) << 4., 5.))));
EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.).finished())));
EXPECT(assert_equal((Vector(2) << 3.,3.).finished(), 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 = (Matrix(1, 2) << 1.0, 1.0);
expectedH = (Matrix(1, 2) << 1.0, 1.0).finished();
EXPECT(assert_equal(expectedH,actualH));
actual = x2.norm(actualH);

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

@ -40,8 +40,8 @@ TEST(Point3, Lie) {
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.).finished())));
EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.).finished(), p1.localCoordinates(p2)));
}
/* ************************************************************************* */

50
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((Vector(3) << 0.01, -0.015, 0.99));
Pose2 actual = pose.retract((Vector(3) << 0.01, -0.015, 0.99).finished());
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, (Vector(3) << 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99).finished());
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, (Vector(3) << 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99).finished());
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -95,11 +95,11 @@ TEST(Pose2, expmap3) {
Matrix A = (Matrix(3,3) <<
0.0, -0.99, 0.01,
0.99, 0.0, -0.015,
0.0, 0.0, 0.0);
0.0, 0.0, 0.0).finished();
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 = (Vector(3) << 0.01, -0.015, 0.99);
Vector v = (Vector(3) << 0.01, -0.015, 0.99).finished();
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((Vector(3) << 0.01, -0.015, 0.018));
Pose2 actual = Pose2::Expmap((Vector(3) << 0.01, -0.015, 0.018).finished());
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((Vector(3) << M_PI/2, 0.0, M_PI/2));
Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2).finished());
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((Vector(3) << M_PI, 0.0, M_PI));
Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI).finished());
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((Vector(3) << 2*M_PI, 0.0, 2*M_PI));
Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI).finished());
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -161,7 +161,7 @@ TEST(Pose3, expmap_c)
TEST(Pose2, expmap_c_full)
{
double w=0.3;
Vector xi = (Vector(3) << 0.0, w, w);
Vector xi = (Vector(3) << 0.0, w, w).finished();
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 = (Vector(3) << 0.00986473, -0.0150896, 0.018);
Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018).finished();
#else
Vector expected = (Vector(3) << 0.01, -0.015, 0.018);
Vector expected = (Vector(3) << 0.01, -0.015, 0.018).finished();
#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 = (Vector(3) << 0.00986473, -0.0150896, 0.018);
Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018).finished();
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 = (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);
Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished();
Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0).finished();
// actual
Matrix actualH1, actualH2;
@ -236,8 +236,8 @@ TEST (Pose2, transform_from)
Point2 expected(0., 2.);
EXPECT(assert_equal(expected, actual));
Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.);
Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished();
Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished();
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt);
EXPECT(assert_equal(H1_expected, H1));
@ -265,7 +265,7 @@ TEST(Pose2, compose_a)
0.0, 1.0, 0.0,
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
);
).finished();
Matrix expectedH2 = eye(3);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
@ -348,7 +348,7 @@ TEST(Pose2, inverse )
namespace {
/* ************************************************************************* */
Vector homogeneous(const Point2& p) {
return (Vector(3) << p.x(), p.y(), 1.0);
return (Vector(3) << p.x(), p.y(), 1.0).finished();
}
/* ************************************************************************* */
@ -358,7 +358,7 @@ namespace {
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);
0.0, 0.0, 1.0).finished();
}
}
@ -371,14 +371,14 @@ TEST( Pose2, matrix )
EXPECT(assert_equal((Matrix(3,3) <<
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
0.0, 0.0, 1.0).finished(),
gMl));
Rot2 gR1 = gTl.r();
EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
EXPECT(assert_equal((Matrix(2,2) <<
0.0, -1.0,
1.0, 0.0),
1.0, 0.0).finished(),
gR1.matrix()));
EXPECT(assert_equal(Point2(0,1),gR1*x_axis));
EXPECT(assert_equal(Point2(-1,0),gR1*y_axis));
@ -390,7 +390,7 @@ TEST( Pose2, matrix )
EXPECT(assert_equal((Matrix(3,3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
0.0, 0.0, 1.0).finished(),
lMg));
}
@ -425,7 +425,7 @@ TEST( Pose2, between )
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
);
).finished();
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1));
@ -436,7 +436,7 @@ TEST( Pose2, between )
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
);
).finished();
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
EXPECT(assert_equal(expectedH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2));

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((Vector(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).finished());
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 = (Vector(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).finished();
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 = (Vector(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).finished();
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 = (Vector(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).finished();
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 = (Vector(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).finished();
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 = (Vector(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).finished();
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 = (Vector(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).finished();
// 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)
@ -674,7 +674,7 @@ TEST(Pose3, align_2) {
/* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*exp(xi+x))
Vector xi = (Vector(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).finished();
Vector testDerivExpmapInv(const Vector6& dxi) {
return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
@ -713,8 +713,8 @@ Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
}
TEST( Pose3, adjointTranspose) {
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 xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0).finished();
Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
Vector expected = testDerivAdjointTranspose(xi, v);
Matrix actualH;

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

@ -96,7 +96,7 @@ TEST(Rot2, logmap)
{
Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = (Vector(1) << M_PI/2.0);
Vector expected = (Vector(1) << M_PI/2.0).finished();
Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual));
}

48
gtsam/geometry/tests/testRot3.cpp
View File

@ -50,7 +50,7 @@ TEST( Rot3, constructor)
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1);
Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
Rot3 actual(R);
Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1);
CHECK(assert_equal(actual,expected));
@ -94,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vector(3) << epsilon, 0., 0.);
Vector w = (Vector(3) << epsilon, 0., 0.).finished();
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
@ -102,7 +102,7 @@ TEST( Rot3, rodriguez)
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y
Vector axis = (Vector(3) << 0., 1., 0.).finished(); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
@ -114,7 +114,7 @@ TEST( Rot3, rodriguez2)
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Vector w = (Vector(3) << 0.1, 0.2, 0.3).finished();
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
@ -123,7 +123,7 @@ TEST( Rot3, rodriguez3)
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
Vector axis = (Vector(3) << 0., 0., 1.).finished(); // rotation around Z
double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
@ -154,7 +154,7 @@ TEST(Rot3, log)
Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
@ -196,7 +196,7 @@ TEST(Rot3, log)
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
@ -256,7 +256,7 @@ TEST(Rot3, manifold_expmap)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished();
// 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)
@ -369,7 +369,7 @@ TEST( Rot3, between )
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);
0.0, 0.0, 1.0).finished();
EXPECT(assert_equal(expectedr1, r1.matrix()));
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
@ -393,7 +393,7 @@ TEST( Rot3, between )
}
/* ************************************************************************* */
Vector w = (Vector(3) << 0.1, 0.27, -0.2);
Vector w = (Vector(3) << 0.1, 0.27, -0.2).finished();
// Left trivialization Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
@ -462,7 +462,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)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3).finished(),expected.ypr()));
}
/* ************************************************************************* */
@ -472,25 +472,25 @@ TEST( Rot3, RQ)
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671).finished();
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)(Vector(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).finished(),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)(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()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3).finished(),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1).finished(),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
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()));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0).finished(),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0).finished(),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1).finished(),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Matrix(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).finished();
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
@ -499,13 +499,13 @@ TEST( Rot3, RQ)
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7).finished();
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
-w(1), w(0), 0.0 ).finished();
Matrix W2 = W*W;
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
@ -515,7 +515,7 @@ TEST( Rot3, expmapStability ) {
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
Vector w = (Vector(3) << 1e-8, 0.0, 0.0).finished();
Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
@ -533,13 +533,13 @@ TEST(Rot3, quaternion) {
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));
-0.769319620053772, 0.637998195662053, 0.033250932803219).finished());
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
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));
0.424630407073532, 0.893945571198514, -0.143353873763946).finished());
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));

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

@ -56,7 +56,7 @@ TEST(Rot3, manifold_cayley)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished();
// 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)
@ -84,7 +84,7 @@ TEST(Rot3, manifold_slow_cayley)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished();
// 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)

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

@ -32,7 +32,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
).finished(),
Point3(0,0,0.5));
static const SimpleCamera camera(pose1, K);
@ -147,7 +147,7 @@ TEST( SimpleCamera, simpleCamera)
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);
7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2).finished();
SimpleCamera actual = simpleCamera(P);
// Note precision of numbers given in book
CHECK(assert_equal(expected, actual,1e-1));

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

@ -78,7 +78,7 @@ static Pose3 camera1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
).finished(),
Point3(0,0,6.25));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));

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((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal((Vector(3) << 3., 3., 3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vector(3) << 4., 5., 6.).finished())));
EXPECT(assert_equal((Vector(3) << 3., 3., 3.).finished(), p1.localCoordinates(p2)));
}
/* ************************************************************************* */

28
gtsam/geometry/tests/testUnit3.cpp
View File

@ -106,11 +106,11 @@ TEST(Unit3, unrotate) {
//*******************************************************************************
TEST(Unit3, error) {
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.717356, 0), p.error(r), 1e-5));
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0).finished()), //
r = p.retract((Vector(2) << 0.8, 0).finished());
EXPECT(assert_equal((Vector(2) << 0, 0).finished(), p.error(p), 1e-8));
EXPECT(assert_equal((Vector(2) << 0.479426, 0).finished(), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.717356, 0).finished(), p.error(r), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
@ -130,8 +130,8 @@ TEST(Unit3, error) {
//*******************************************************************************
TEST(Unit3, distance) {
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0).finished()), //
r = p.retract((Vector(2) << 0.8, 0).finished());
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);
@ -169,7 +169,7 @@ TEST(Unit3, localCoordinates1) {
//*******************************************************************************
TEST(Unit3, localCoordinates2) {
Unit3 p, q(-1, 0, 0);
Vector expected = (Vector(2) << M_PI, 0);
Vector expected = (Vector(2) << M_PI, 0).finished();
Vector actual = p.localCoordinates(q);
CHECK(assert_equal(expected, actual, 1e-8));
}
@ -228,9 +228,9 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(Unit3, localCoordinates_retract) {
size_t numIterations = 10000;
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0);
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0).finished(), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0).finished();
Vector minXiLimit = (Vector(2) << -1.0, -1.0).finished(), maxXiLimit = (Vector(2) << 1.0, 1.0).finished();
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
@ -258,9 +258,9 @@ TEST(Unit3, localCoordinates_retract) {
TEST(Unit3, localCoordinates_retract_expmap) {
size_t numIterations = 10000;
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI);
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0).finished(), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0).finished();
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished();
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).

8
gtsam/linear/VectorValues.h
View File

@ -49,15 +49,15 @@ namespace gtsam {
* Example:
* \code
VectorValues values;
values.insert(3, (Vector(3) << 1.0, 2.0, 3.0));
values.insert(4, (Vector(2) << 4.0, 5.0));
values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0));
values.insert(3, (Vector(3) << 1.0, 2.0, 3.0).finished());
values.insert(4, (Vector(2) << 4.0, 5.0).finished());
values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
// Prints [ 8.0 9.0 ]
values[1] = (Vector(2) << 8.0, 9.0);
values[1] = (Vector(2) << 8.0, 9.0).finished();
gtsam::print(values[1]);
\endcode
*

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

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

68
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

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

108
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -37,10 +37,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, (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));
(JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise));
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
/* ************************************************************************* */
@ -83,13 +83,13 @@ TEST( GaussianBayesTree, eliminate )
{
GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering);
Matrix two = (Matrix(1, 1) << 2.);
Matrix one = (Matrix(1, 1) << 1.);
Matrix two = (Matrix(1, 1) << 2.).finished();
Matrix one = (Matrix(1, 1) << 1.).finished();
GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot(
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.))))));
MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, (Vector(2) << 2., 2.).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished())))));
EXPECT(assert_equal(bayesTree_expected, bt));
}
@ -98,10 +98,10 @@ TEST( GaussianBayesTree, eliminate )
TEST( GaussianBayesTree, optimizeMultiFrontal )
{
VectorValues expected = pair_list_of<Key, Vector>
(x1, (Vector(1) << 0.))
(x2, (Vector(1) << 1.))
(x3, (Vector(1) << 0.))
(x4, (Vector(1) << 1.));
(x1, (Vector(1) << 0.).finished())
(x2, (Vector(1) << 1.).finished())
(x3, (Vector(1) << 0.).finished())
(x4, (Vector(1) << 1.).finished());
VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
EXPECT(assert_equal(expected,actual));
@ -113,39 +113,39 @@ TEST(GaussianBayesTree, complicatedMarginal) {
// Create the conditionals to go in the BayesTree
GaussianBayesTree bt;
bt.insertRoot(
MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (11, (Matrix(3,1) << 0.0971, 0, 0))
(12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655)),
2, (Vector(3) << 0.2638, 0.1455, 0.1361)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (9, (Matrix(3,1) << 0.7952, 0, 0))
(10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797))
(11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190))
(12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)),
2, (Vector(3) << 0.4314, 0.9106, 0.1818))))
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (7, (Matrix(3,1) << 0.2551, 0, 0))
(8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575))
(11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143)),
2, (Vector(3) << 0.3998, 0.2599, 0.8001)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (5, (Matrix(3,1) << 0.2435, 0, 0))
(6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0))
MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (11, (Matrix(3,1) << 0.0971, 0, 0).finished())
(12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()),
2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (9, (Matrix(3,1) << 0.7952, 0, 0).finished())
(10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished())
(11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished())
(12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()),
2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished())))
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (7, (Matrix(3,1) << 0.2551, 0, 0).finished())
(8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished())
(11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()),
2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (5, (Matrix(3,1) << 0.2435, 0, 0).finished())
(6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished())
// NOTE the non-upper-triangular form
// here since this test was written when we had column permutations
// from LDL. The code still works currently (does not enfore
// upper-triangularity in this case) but this test will need to be
// redone if this stops working in the future
(7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172))
(8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)),
2, (Vector(3) << 0.8173, 0.8687, 0.0844)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (3, (Matrix(3,1) << 0.0540, 0, 0))
(4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371))
(6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)),
2, (Vector(3) << 0.9619, 0.0046, 0.7749))))
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (1, (Matrix(3,1) << 0.2630, 0, 0))
(2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524))
(5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961)),
2, (Vector(3) << 0.0782, 0.4427, 0.1067))))))))));
(7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished())
(8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()),
2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (3, (Matrix(3,1) << 0.0540, 0, 0).finished())
(4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished())
(6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()),
2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished())))
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (1, (Matrix(3,1) << 0.2630, 0, 0).finished())
(2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished())
(5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()),
2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished())))))))));
// Marginal on 5
Matrix expectedCov = (Matrix(1,1) << 236.5166);
Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
//GaussianConditional actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky);
GaussianConditional actualJacobianQR = *bt.marginalFactor(5, EliminateQR);
//EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
@ -162,7 +162,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
// 2886.2, 1015.8);
expectedCov = (Matrix(2,2) <<
1015.8, 2886.2,
2886.2, 8471.2);
2886.2, 8471.2).finished();
//actualJacobianChol = bt.marginalFactor(6, EliminateCholesky);
actualJacobianQR = *bt.marginalFactor(6, EliminateQR);
//EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
@ -196,50 +196,50 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
0.0,0.0,
0.0,0.0,
0.0,0.0,
0.0,0.0))
0.0,0.0).finished())
(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))
0.0,0.0).finished())
(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, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of
0.0,54.0).finished()),
3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0).finished()), list_of
(MakeClique(GaussianConditional(
pair_list_of<Key, Matrix>
(0, (Matrix(4, 2) <<
3.0,4.0,
0.0,6.0,
0.0,0.0,
0.0,0.0))
0.0,0.0).finished())
(1, (Matrix(4, 2) <<
0.0,0.0,
0.0,0.0,
17.0,18.0,
0.0,20.0))
0.0,20.0).finished())
(2, (Matrix(4, 2) <<
0.0,0.0,
0.0,0.0,
21.0,22.0,
23.0,24.0))
23.0,24.0).finished())
(3, (Matrix(4, 2) <<
7.0,8.0,
9.0,10.0,
0.0,0.0,
0.0,0.0))
0.0,0.0).finished())
(4, (Matrix(4, 2) <<
11.0,12.0,
13.0,14.0,
25.0,26.0,
27.0,28.0)),
2, (Vector(4) << 1.0,2.0,15.0,16.0))))));
27.0,28.0).finished()),
2, (Vector(4) << 1.0,2.0,15.0,16.0).finished())))));
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(
@ -261,11 +261,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Known steepest descent point from Bayes' net version
VectorValues expectedFromBN = pair_list_of<Key, Vector>
(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));
(0, (Vector(2) << 0.000129034, 0.000688183).finished())
(1, (Vector(2) << 0.0109679, 0.0253767).finished())
(2, (Vector(2) << 0.0680441, 0.114496).finished())
(3, (Vector(2) << 0.16125, 0.241294).finished())
(4, (Vector(2) << 0.300134, 0.423233).finished());
// Compute the steepest descent point with the dogleg function
VectorValues actual = bt.optimizeGradientSearch();

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

@ -43,23 +43,23 @@ static const double tol = 1e-5;
static Matrix R = (Matrix(2, 2) <<
-12.1244, -5.1962,
0., 4.6904);
0., 4.6904).finished();
/* ************************************************************************* */
TEST(GaussianConditional, constructor)
{
Matrix S1 = (Matrix(2, 2) <<
-5.2786, -8.6603,
5.0254, 5.5432);
5.0254, 5.5432).finished();
Matrix S2 = (Matrix(2, 2) <<
-10.5573, -5.9385,
5.5737, 3.0153);
5.5737, 3.0153).finished();
Matrix S3 = (Matrix(2, 2) <<
-11.3820, -7.2581,
-3.0153, -3.5635);
-3.0153, -3.5635).finished();
Vector d = (Vector(2) << 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0));
Vector d = (Vector(2) << 1.0, 2.0).finished();
SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0).finished());
vector<pair<Key, Matrix> > terms = pair_list_of
(1, R)
@ -116,9 +116,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((Vector(2) << 1.0, 0.34));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 0.34).finished());
Vector d = (Vector(2) << 0.2, 0.5);
Vector d = (Vector(2) << 0.2, 0.5).finished();
GaussianConditional
expected(1, d, R, 2, A1, 10, A2, model),
@ -136,13 +136,13 @@ TEST( GaussianConditional, solve )
// create a conditional Gaussian node
Matrix R = (Matrix(2, 2) << 1., 0.,
0., 1.);
0., 1.).finished();
Matrix A1 = (Matrix(2, 2) << 1., 2.,
3., 4.);
3., 4.).finished();
Matrix A2 = (Matrix(2, 2) << 5., 6.,
7., 8.);
7., 8.).finished();
Vector d(2); d << 20.0, 40.0;
@ -179,7 +179,7 @@ TEST( GaussianConditional, solve_simple )
GaussianConditional cg(list_of(1)(2), 1, blockMatrix);
// partial solution
Vector sx1 = (Vector(2) << 9.0, 10.0);
Vector sx1 = (Vector(2) << 9.0, 10.0).finished();
// elimination order: 1, 2
VectorValues actual = map_list_of
@ -187,7 +187,7 @@ TEST( GaussianConditional, solve_simple )
VectorValues expected = map_list_of<Key, Vector>
(2, sx1)
(1, (Vector(4) << -3.1,-3.4,-11.9,-13.2));
(1, (Vector(4) << -3.1,-3.4,-11.9,-13.2).finished());
// verify indices/size
EXPECT_LONGS_EQUAL(2, (long)cg.size());
@ -215,15 +215,15 @@ TEST( GaussianConditional, solve_multifrontal )
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d()));
// partial solution
Vector sl1 = (Vector(2) << 9.0, 10.0);
Vector sl1 = (Vector(2) << 9.0, 10.0).finished();
// elimination order; _x_, _x1_, _l1_
VectorValues actual = map_list_of
(10, sl1); // parent
VectorValues expected = map_list_of<Key, Vector>
(1, (Vector)(Vector(2) << -3.1,-3.4))
(2, (Vector)(Vector(2) << -11.9,-13.2))
(1, (Vector(2) << -3.1,-3.4).finished())
(2, (Vector(2) << -11.9,-13.2).finished())
(10, sl1);
// verify indices/size
@ -243,8 +243,8 @@ TEST( GaussianConditional, solveTranspose ) {
* 1 1 9
* 1 5
*/
Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0);
Matrix R22 = (Matrix(1, 1) << 1.0);
Matrix R11 = (Matrix(1, 1) << 1.0).finished(), S12 = (Matrix(1, 1) << 1.0).finished();
Matrix R22 = (Matrix(1, 1) << 1.0).finished();
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
@ -260,11 +260,11 @@ TEST( GaussianConditional, solveTranspose ) {
VectorValues
x = map_list_of<Key, Vector>
(1, (Vector(1) << 2.))
(2, (Vector(1) << 5.)),
(1, (Vector(1) << 2.).finished())
(2, (Vector(1) << 5.).finished()),
y = map_list_of<Key, Vector>
(1, (Vector(1) << 2.))
(2, (Vector(1) << 3.));
(1, (Vector(1) << 2.).finished())
(2, (Vector(1) << 3.).finished());
// test functional version
VectorValues actual = cbn.backSubstituteTranspose(x);

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

@ -27,9 +27,9 @@ TEST(GaussianDensity, constructor)
{
Matrix R = (Matrix(2,2) <<
-12.1244, -5.1962,
0., 4.6904);
0., 4.6904).finished();
Vector d = (Vector(2) << 1.0, 2.0), s = (Vector(2) << 3.0, 4.0);
Vector d = (Vector(2) << 1.0, 2.0).finished(), s = (Vector(2) << 3.0, 4.0).finished();
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
GaussianDensity copied(conditional);

90
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -48,9 +48,9 @@ TEST(GaussianFactorGraph, initialization) {
fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), 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);
JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0).finished(), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0).finished(), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -60,7 +60,7 @@ TEST(GaussianFactorGraph, initialization) {
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
);
).finished();
Matrix actualIJS = fg.sparseJacobian_();
EQUALITY(expectedIJS, actualIJS);
}
@ -91,14 +91,14 @@ TEST(GaussianFactorGraph, sparseJacobian) {
4., 4.,28.,
4., 5.,30.,
3., 6.,26.,
4., 6.,32.);
4., 6.,32.).finished();
Matrix expected = expectedT.transpose();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
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);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), (Vector(2) << 4., 8.).finished(), model);
gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.).finished(), 1, (Matrix(2, 2) << 11., 12., 14., 15.).finished(), (Vector(2) << 13.,16.).finished(), model);
Matrix actual = gfg.sparseJacobian_();
@ -114,14 +114,14 @@ TEST(GaussianFactorGraph, matrices) {
// 9 10 0 11 12 13
// 0 0 0 14 15 16
Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7);
Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0);
Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15);
Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, A00, (Vector(2) << 4., 8.), model);
gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model);
gfg.add(0, A00, (Vector(2) << 4., 8.).finished(), model);
gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.).finished(), model);
Matrix Ab(4,6);
Ab <<
@ -150,8 +150,8 @@ TEST(GaussianFactorGraph, matrices) {
// hessianBlockDiagonal
VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49));
expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225));
expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49).finished());
expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225).finished());
EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
// hessianBlockDiagonal
@ -168,11 +168,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(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0).finished(), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0).finished(), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2);
return fg;
}
@ -185,9 +185,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, (Vector(2) << 5.0, -12.5))
(2, (Vector(2) << 30.0, 5.0))
(0, (Vector(2) << -25.0, 17.5));
(1, (Vector(2) << 5.0, -12.5).finished())
(2, (Vector(2) << 30.0, 5.0).finished())
(0, (Vector(2) << -25.0, 17.5).finished());
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
@ -207,15 +207,15 @@ TEST( GaussianFactorGraph, transposeMultiplication )
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
Errors e; e +=
(Vector(2) << 0.0, 0.0),
(Vector(2) << 15.0, 0.0),
(Vector(2) << 0.0,-5.0),
(Vector(2) << -7.5,-5.0);
(Vector(2) << 0.0, 0.0).finished(),
(Vector(2) << 15.0, 0.0).finished(),
(Vector(2) << 0.0,-5.0).finished(),
(Vector(2) << -7.5,-5.0).finished();
VectorValues expected;
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));
expected.insert(1, (Vector(2) << -37.5,-50.0).finished());
expected.insert(2, (Vector(2) << -150.0, 25.0).finished());
expected.insert(0, (Vector(2) << 187.5, 25.0).finished());
VectorValues actual = A.transposeMultiply(e);
EXPECT(assert_equal(expected, actual));
@ -259,14 +259,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
VectorValues x = map_list_of<Key, Vector>
(0, (Vector(2) << 1,2))
(1, (Vector(2) << 3,4))
(2, (Vector(2) << 5,6));
(0, (Vector(2) << 1,2).finished())
(1, (Vector(2) << 3,4).finished())
(2, (Vector(2) << 5,6).finished());
VectorValues expected;
expected.insert(0, (Vector(2) << -450, -450));
expected.insert(1, (Vector(2) << 0, 0));
expected.insert(2, (Vector(2) << 950, 1050));
expected.insert(0, (Vector(2) << -450, -450).finished());
expected.insert(1, (Vector(2) << 0, 0).finished());
expected.insert(2, (Vector(2) << 950, 1050).finished());
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
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);
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vector(2) << 0.0, 1.0).finished(),
400*eye(2,2), (Vector(2) << 1.0, 1.0).finished(), 3.0);
return gfg;
}
@ -297,14 +297,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
EXPECT(assert_equal(Y,AtA*X));
VectorValues x = map_list_of<Key, Vector>
(0, (Vector(2) << 1,2))
(1, (Vector(2) << 3,4))
(2, (Vector(2) << 5,6));
(0, (Vector(2) << 1,2).finished())
(1, (Vector(2) << 3,4).finished())
(2, (Vector(2) << 5,6).finished());
VectorValues expected;
expected.insert(0, (Vector(2) << -450, -450));
expected.insert(1, (Vector(2) << 300, 400));
expected.insert(2, (Vector(2) << 2950, 3450));
expected.insert(0, (Vector(2) << -450, -450).finished());
expected.insert(1, (Vector(2) << 300, 400).finished());
expected.insert(2, (Vector(2) << 2950, 3450).finished());
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
@ -346,7 +346,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 = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4);
Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
EXPECT(assert_equal(expected, eta));
EXPECT(assert_equal(A.transpose()*b, eta));
}
@ -358,9 +358,9 @@ TEST( GaussianFactorGraph, gradientAtZero )
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
VectorValues actual = gfg.gradientAtZero();
expected.insert(0, (Vector(2) << -25, 17.5));
expected.insert(1, (Vector(2) << 5, -13.5));
expected.insert(2, (Vector(2) << 29, 4));
expected.insert(0, (Vector(2) << -25, 17.5).finished());
expected.insert(1, (Vector(2) << 5, -13.5).finished());
expected.insert(2, (Vector(2) << 29, 4).finished());
EXPECT(assert_equal(expected, actual));
}

144
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -59,25 +59,25 @@ TEST(HessianFactor, ConversionConstructor)
0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000,
-100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000,
25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500)));
25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500).finished()));
JacobianFactor jacobian(
0, (Matrix(4,2) << -1., 0.,
+0.,-1.,
1., 0.,
+0.,1.),
+0.,1.).finished(),
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
(Vector(4) << -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1)));
0., 0., 0.00, -1.).finished(), // f2
(Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(),
noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1).finished()));
HessianFactor actual(jacobian);
VectorValues values = pair_list_of<Key, Vector>
(0, (Vector(2) << 1.0, 2.0))
(1, (Vector(4) << 3.0, 4.0, 5.0, 6.0));
(0, (Vector(2) << 1.0, 2.0).finished())
(1, (Vector(4) << 3.0, 4.0, 5.0, 6.0).finished());
EXPECT_LONGS_EQUAL(2, (long)actual.size());
EXPECT(assert_equal(expected, actual, 1e-9));
@ -87,8 +87,8 @@ TEST(HessianFactor, ConversionConstructor)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1)
{
Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g = (Vector(2) << -8.0, -9.0);
Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
Vector g = (Vector(2) << -8.0, -9.0).finished();
double f = 10.0;
HessianFactor factor(0, G, g, f);
@ -98,7 +98,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, (Vector(2) << 1.5, 2.5));
VectorValues dx = pair_list_of<Key, Vector>(0, (Vector(2) << 1.5, 2.5).finished());
// error 0.5*(f - 2*x'*g + x'*G*x)
double expected = 80.375;
@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1b)
{
Vector mu = (Vector(2) << 1.0,2.0);
Vector mu = (Vector(2) << 1.0,2.0).finished();
Matrix Sigma = eye(2,2);
HessianFactor factor(0, mu, Sigma);
@ -131,15 +131,15 @@ TEST(HessianFactor, Constructor1b)
/* ************************************************************************* */
TEST(HessianFactor, Constructor2)
{
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);
Matrix G11 = (Matrix(1,1) << 1.0).finished();
Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
Vector g1 = (Vector(1) << -7.0).finished();
Vector g2 = (Vector(2) << -8.0, -9.0).finished();
double f = 10.0;
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
Vector dx0 = (Vector(1) << 0.5).finished();
Vector dx1 = (Vector(2) << 1.5, 2.5).finished();
VectorValues dx = pair_list_of
(0, dx0)
@ -165,31 +165,31 @@ TEST(HessianFactor, Constructor2)
VectorValues dxLarge = pair_list_of<Key, Vector>
(0, dx0)
(1, dx1)
(2, (Vector(2) << 0.1, 0.2));
(2, (Vector(2) << 0.1, 0.2).finished());
EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
}
/* ************************************************************************* */
TEST(HessianFactor, Constructor3)
{
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 G11 = (Matrix(1,1) << 1.0).finished();
Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();
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 G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
Matrix G33 = (Matrix(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).finished();
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0).finished();
Vector g2 = (Vector(2) << -8.0, -9.0).finished();
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished();
double f = 10.0;
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5);
Vector dx0 = (Vector(1) << 0.5).finished();
Vector dx1 = (Vector(2) << 1.5, 2.5).finished();
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5).finished();
VectorValues dx = pair_list_of
(0, dx0)
@ -219,24 +219,24 @@ TEST(HessianFactor, Constructor3)
/* ************************************************************************* */
TEST(HessianFactor, ConstructorNWay)
{
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 G11 = (Matrix(1,1) << 1.0).finished();
Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();
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 G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
Matrix G33 = (Matrix(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).finished();
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0).finished();
Vector g2 = (Vector(2) << -8.0, -9.0).finished();
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished();
double f = 10.0;
Vector dx0 = (Vector(1) << 0.5);
Vector dx1 = (Vector(2) << 1.5, 2.5);
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5);
Vector dx0 = (Vector(1) << 0.5).finished();
Vector dx1 = (Vector(2) << 1.5, 2.5).finished();
Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5).finished();
VectorValues dx = pair_list_of
(0, dx0)
@ -275,25 +275,25 @@ TEST(HessianFactor, CombineAndEliminate)
Matrix A01 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
0.0, 0.0, 1.0).finished();
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
0.0, 0.0, 2.0).finished();
Matrix A11 = (Matrix(3,3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
0.0, 0.0, -2.0).finished();
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
0.0, 0.0, 3.0).finished();
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
@ -331,7 +331,7 @@ TEST(HessianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();
// the combined linear factor
Matrix Ax2 = (Matrix(4,2) <<
@ -340,7 +340,7 @@ TEST(HessianFactor, eliminate2 )
+0.,-1.,
1., 0.,
+0.,1.
);
).finished();
Matrix Al1x1 = (Matrix(4,4) <<
// l1 x1
@ -348,7 +348,7 @@ TEST(HessianFactor, eliminate2 )
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00,-1. // f2
);
).finished();
// the RHS
Vector b2(4);
@ -374,12 +374,12 @@ TEST(HessianFactor, eliminate2 )
Matrix R11 = (Matrix(2,2) <<
1.00, 0.00,
0.00, 1.00
)/oldSigma;
).finished()/oldSigma;
Matrix S12 = (Matrix(2,4) <<
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14)/oldSigma;
).finished()/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14).finished()/oldSigma;
GaussianConditional expectedCG(0, d, R11, 1, S12);
EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
@ -389,8 +389,8 @@ TEST(HessianFactor, eliminate2 )
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = (Vector(2) << 0.0,0.894427);
).finished()/sigma;
Vector b1 = (Vector(2) << 0.0,0.894427).finished();
JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
}
@ -401,14 +401,14 @@ TEST(HessianFactor, combine) {
// update the information matrix with a single jacobian factor
Matrix A0 = (Matrix(2, 2) <<
11.1803399, 0.0,
0.0, 11.1803399);
0.0, 11.1803399).finished();
Matrix A1 = (Matrix(2, 2) <<
-2.23606798, 0.0,
0.0, -2.23606798);
0.0, -2.23606798).finished();
Matrix A2 = (Matrix(2, 2) <<
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = (Vector(2) << 2.23606798,-1.56524758);
0.0, -8.94427191).finished();
Vector b = (Vector(2) << 2.23606798,-1.56524758).finished();
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);
@ -423,7 +423,7 @@ TEST(HessianFactor, combine) {
0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
-100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500);
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500).finished();
EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol));
}
@ -431,11 +431,11 @@ TEST(HessianFactor, combine) {
/* ************************************************************************* */
TEST(HessianFactor, gradientAtZero)
{
Matrix G11 = (Matrix(1, 1) << 1);
Matrix G12 = (Matrix(1, 2) << 0, 0);
Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1);
Vector g1 = (Vector(1) << -7);
Vector g2 = (Vector(2) << -8, -9);
Matrix G11 = (Matrix(1, 1) << 1).finished();
Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
Vector g1 = (Vector(1) << -7).finished();
Vector g2 = (Vector(2) << -8, -9).finished();
double f = 194;
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
@ -452,19 +452,19 @@ TEST(HessianFactor, gradientAtZero)
/* ************************************************************************* */
TEST(HessianFactor, hessianDiagonal)
{
Matrix G11 = (Matrix(1, 1) << 1);
Matrix G12 = (Matrix(1, 2) << 0, 0);
Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1);
Vector g1 = (Vector(1) << -7);
Vector g2 = (Vector(2) << -8, -9);
Matrix G11 = (Matrix(1, 1) << 1).finished();
Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
Vector g1 = (Vector(1) << -7).finished();
Vector g2 = (Vector(2) << -8, -9).finished();
double f = 194;
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
// hessianDiagonal
VectorValues expected;
expected.insert(0, (Vector(1) << 1));
expected.insert(1, (Vector(2) << 1,1));
expected.insert(0, (Vector(1) << 1).finished());
expected.insert(1, (Vector(2) << 1,1).finished());
EXPECT(assert_equal(expected, factor.hessianDiagonal()));
// hessianBlockDiagonal

72
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -42,8 +42,8 @@ namespace {
(make_pair(15, 3*Matrix3::Identity()));
// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5));
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5).finished());
}
}
@ -149,14 +149,14 @@ TEST(JabobianFactor, Hessian_conversion) {
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675),
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);
JacobianFactor expected(0, (Matrix(2,4) <<
1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943),
(Vector(2) << -6.2929, -5.7941));
0, 2.5858, 0.4789, -2.3943).finished(),
(Vector(2) << -6.2929, -5.7941).finished());
EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
}
@ -296,9 +296,9 @@ TEST(JacobianFactor, matrices)
// hessianDiagonal
VectorValues expectDiagonal;
// below we divide by the variance 0.5^2
expectDiagonal.insert(5, (Vector(3) << 1, 1, 1)/0.25);
expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25);
expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25);
expectDiagonal.insert(5, (Vector(3) << 1, 1, 1).finished()/0.25);
expectDiagonal.insert(10, (Vector(3) << 4, 4, 4).finished()/0.25);
expectDiagonal.insert(15, (Vector(3) << 9, 9, 9).finished()/0.25);
EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
// hessianBlockDiagonal
@ -315,22 +315,22 @@ TEST(JacobianFactor, operators )
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Vector b = (Vector(2) << 0.2,-0.1);
Vector b = (Vector(2) << 0.2,-0.1).finished();
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
VectorValues c;
c.insert(1, (Vector(2) << 10.,20.));
c.insert(2, (Vector(2) << 30.,60.));
c.insert(1, (Vector(2) << 10.,20.).finished());
c.insert(2, (Vector(2) << 30.,60.).finished());
// test A*x
Vector expectedE = (Vector(2) << 200.,400.);
Vector expectedE = (Vector(2) << 200.,400.).finished();
Vector actualE = lf * c;
EXPECT(assert_equal(expectedE, actualE));
// test A^e
VectorValues expectedX;
expectedX.insert(1, (Vector(2) << -2000.,-4000.));
expectedX.insert(2, (Vector(2) << 2000., 4000.));
expectedX.insert(1, (Vector(2) << -2000.,-4000.).finished());
expectedX.insert(2, (Vector(2) << 2000., 4000.).finished());
VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX));
@ -338,8 +338,8 @@ TEST(JacobianFactor, operators )
// test gradient at zero
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
VectorValues expectedG;
expectedG.insert(1, (Vector(2) << 20,-10));
expectedG.insert(2, (Vector(2) << -20, 10));
expectedG.insert(1, (Vector(2) << 20,-10).finished());
expectedG.insert(2, (Vector(2) << -20, 10).finished());
FastVector<Key> keys; keys += 1,2;
EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
VectorValues actualG = lf.gradientAtZero();
@ -368,25 +368,25 @@ TEST(JacobianFactor, eliminate)
Matrix A01 = (Matrix(3, 3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
0.0, 0.0, 1.0).finished();
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3, 3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
0.0, 0.0, 2.0).finished();
Matrix A11 = (Matrix(3, 3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
0.0, 0.0, -2.0).finished();
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3, 3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
0.0, 0.0, 3.0).finished();
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
@ -420,7 +420,7 @@ TEST(JacobianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();
// the combined linear factor
Matrix Ax2 = (Matrix(4, 2) <<
@ -429,7 +429,7 @@ TEST(JacobianFactor, eliminate2 )
+0.,-1.,
1., 0.,
+0.,1.
);
).finished();
Matrix Al1x1 = (Matrix(4, 4) <<
// l1 x1
@ -437,7 +437,7 @@ TEST(JacobianFactor, eliminate2 )
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00,-1. // f2
);
).finished();
// the RHS
Vector b2(4);
@ -460,12 +460,12 @@ TEST(JacobianFactor, eliminate2 )
Matrix R11 = (Matrix(2, 2) <<
1.00, 0.00,
0.00, 1.00
)/oldSigma;
).finished()/oldSigma;
Matrix S12 = (Matrix(2, 4) <<
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14)/oldSigma;
).finished()/oldSigma;
Vector d = (Vector(2) << 0.2,-0.14).finished()/oldSigma;
GaussianConditional expectedCG(2, d, R11, 11, S12);
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
@ -476,8 +476,8 @@ TEST(JacobianFactor, eliminate2 )
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = (Vector(2) << 0.0, 0.894427);
).finished()/sigma;
Vector b1 = (Vector(2) << 0.0, 0.894427).finished();
JacobianFactor expectedLF(11, Bl1x1, b1);
EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
}
@ -500,7 +500,7 @@ TEST(JacobianFactor, EliminateQR)
0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3.,
0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5.,
0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.);
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.).finished();
// Create factor graph
const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5);
@ -527,7 +527,7 @@ TEST(JacobianFactor, EliminateQR)
0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635);
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished();
GaussianConditional expectedFragment(
list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R));
@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
EXPECT(actual.second->size() == 0);
// verify conditional Gaussian
Vector sigmas = (Vector(2) << 0.0, 0.0);
Vector sigmas = (Vector(2) << 0.0, 0.0).finished();
GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first));
}
@ -600,12 +600,12 @@ TEST ( JacobianFactor, constraint_eliminate2 )
// verify CG
Matrix R = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 1.0);
0.0, 1.0).finished();
Matrix S = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 0.0);
Vector d = (Vector(2) << 3.0, 0.6666);
Vector sigmas = (Vector(2) << 0.0, 0.0);
0.0, 0.0).finished();
Vector d = (Vector(2) << 3.0, 0.6666).finished();
Vector sigmas = (Vector(2) << 0.0, 0.0).finished();
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((Vector(2) << x, y)) {
Vector((Vector(2) << x, y).finished()) {
}
};
@ -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 = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01);
Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished();
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 = (Vector(2) << 1.0, 0.0);
Vector u = (Vector(2) << 1.0, 0.0).finished();
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 = (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 F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1).finished();
Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8).finished();
Vector u = (Vector(3) << 1.0, 0.0, 2.0).finished();
Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished();
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
@ -177,7 +177,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, -0.6, 10.1, 61.1, 0.0, 0.0, 0.0,
0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, 0.0,
63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0,
-0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0);
-0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0).finished();
// Create two Kalman filter of dimension 9, one using QR the other Cholesky
KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::CHOLESKY);
@ -196,7 +196,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0);
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished();
Matrix B = zeros(9, 1);
Vector u = zero(1);
Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
@ -208,7 +208,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2);
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2).finished();
// Do prediction step
KalmanFilter::State pa = kfa.predictQ(p0a, Psi_k, B, u, dt_Q_k);
@ -219,7 +219,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa->information(), pb->information(), 1e-7));
// and in addition attain the correct covariance
Vector expectedMean = (Vector(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.).finished();
EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7));
EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7));
Matrix expected = 1e-6 * (Matrix(9, 9) <<
@ -231,7 +231,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.2, 1.2, -0.6, 10.1, 61.3, 0.0, 0.0, 0.0,
0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, 0.0,
63.8, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0,
-0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2);
-0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2).finished();
EXPECT(assert_equal(expected, pa->covariance(), 1e-7));
EXPECT(assert_equal(expected, pb->covariance(), 1e-7));
@ -239,9 +239,9 @@ TEST( KalmanFilter, QRvsCholesky ) {
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 = (Vector(3) << 0.2599 , 1.3327 , 0.2007);
Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904);
-83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.).finished();
Vector z = (Vector(3) << 0.2599 , 1.3327 , 0.2007).finished();
Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904).finished();
SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
// do update
@ -253,7 +253,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7));
// and in addition attain the correct mean and covariance
Vector expectedMean2 = (Vector(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).finished();
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 * (Matrix(9, 9) <<
@ -265,7 +265,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.2, 1.2, -0.6, 10.1, 61.3, -0.0, 0.0, 0.0,
0.0, -64.0, -0.0, -0.0, -0.0, -0.0, 647.2, -0.0, 0.0,
63.9, -0.0, 0.1, -0.0, -0.0, 0.0, -0.0, 647.2, 0.1,
-0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8);
-0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8).finished();
EXPECT(assert_equal(expected2, pa2->covariance(), 1e-7));
EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));

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

@ -37,28 +37,28 @@ static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
static Matrix R = (Matrix(3, 3) <<
s_1, 0.0, 0.0,
0.0, s_1, 0.0,
0.0, 0.0, s_1);
0.0, 0.0, s_1).finished();
static Matrix Sigma = (Matrix(3, 3) <<
var, 0.0, 0.0,
0.0, var, 0.0,
0.0, 0.0, var);
0.0, 0.0, var).finished();
//static double inf = numeric_limits<double>::infinity();
/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
Vector whitened = (Vector(3) << 5.0,10.0,15.0);
Vector unwhitened = (Vector(3) << 10.0,20.0,30.0);
Vector whitened = (Vector(3) << 5.0,10.0,15.0).finished();
Vector unwhitened = (Vector(3) << 10.0,20.0,30.0).finished();
// 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((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(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()));
m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished()));
m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished()));
m.push_back(Isotropic::Sigma(3, sigma));
m.push_back(Isotropic::Variance(3, var));
m.push_back(Isotropic::Precision(3, prc));
@ -80,7 +80,7 @@ TEST(NoiseModel, constructors)
Matrix expectedR((Matrix(3, 3) <<
s_1, 0.0, 0.0,
0.0, s_1, 0.0,
0.0, 0.0, s_1));
0.0, 0.0, s_1).finished());
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
EXPECT(assert_equal(expectedR,mi->R()));
@ -89,12 +89,12 @@ TEST(NoiseModel, constructors)
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));
1.0, 0.0, 0.0, 1.0).finished());
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));
s_1, 0.0, 0.0, s_1).finished());
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
EXPECT(assert_equal(expected,mi->Whiten(H)));
@ -107,7 +107,7 @@ TEST(NoiseModel, constructors)
/* ************************************************************************* */
TEST(NoiseModel, Unit)
{
Vector v = (Vector(3) << 5.0,10.0,15.0);
Vector v = (Vector(3) << 5.0,10.0,15.0).finished();
Gaussian::shared_ptr u(Unit::Create(3));
EXPECT(assert_equal(v,u->whiten(v)));
}
@ -117,8 +117,8 @@ TEST(NoiseModel, equals)
{
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)),
d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()),
d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished());
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
i2 = Isotropic::Sigma(3, 0.7);
@ -155,8 +155,8 @@ TEST(NoiseModel, ConstrainedConstructors )
Constrained::shared_ptr actual;
size_t d = 3;
double m = 100.0;
Vector sigmas = (Vector(3) << sigma, 0.0, 0.0);
Vector mu = (Vector(3) << 200.0, 300.0, 400.0);
Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished();
Vector mu = (Vector(3) << 200.0, 300.0, 400.0).finished();
actual = Constrained::All(d);
// TODO: why should this be a thousand ??? Dummy variable?
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
@ -180,12 +180,12 @@ TEST(NoiseModel, ConstrainedConstructors )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
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));
Vector feasible = (Vector(3) << 1.0, 0.0, 1.0).finished(),
infeasible = (Vector(3) << 1.0, 1.0, 1.0).finished();
Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished());
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
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)));
EXPECT(assert_equal((Vector(3) << 0.5, 1.0, 0.5).finished(),d->whiten(infeasible)));
EXPECT(assert_equal((Vector(3) << 0.5, 0.0, 0.5).finished(),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);
@ -194,13 +194,13 @@ TEST(NoiseModel, ConstrainedMixed )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll )
{
Vector feasible = (Vector(3) << 0.0, 0.0, 0.0),
infeasible = (Vector(3) << 1.0, 1.0, 1.0);
Vector feasible = (Vector(3) << 0.0, 0.0, 0.0).finished(),
infeasible = (Vector(3) << 1.0, 1.0, 1.0).finished();
Constrained::shared_ptr i = Constrained::All(3);
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
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)));
EXPECT(assert_equal((Vector(3) << 1.0, 1.0, 1.0).finished(),i->whiten(infeasible)));
EXPECT(assert_equal((Vector(3) << 0.0, 0.0, 0.0).finished(),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
@ -213,15 +213,15 @@ namespace exampleQR {
-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 = (Vector(4) << 0.2, 0.2, 0.1, 0.1);
0., 1., 0., 0., 0., -1., -0.1).finished();
Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished();
// the matrix AB yields the following factorized version:
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,
0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427);
0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427).finished();
SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
}
@ -232,7 +232,7 @@ TEST( NoiseModel, QR )
Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
// Expected result
Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Gaussian version
@ -249,7 +249,7 @@ TEST( NoiseModel, QR )
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,
0., 0., 0., 1., 0., -1., 0.2);
0., 0., 0., 1., 0., -1., 0.2).finished();
EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
}
@ -257,10 +257,10 @@ TEST( NoiseModel, QR )
TEST(NoiseModel, QRNan )
{
SharedDiagonal constrained = noiseModel::Constrained::All(2);
Matrix Ab = (Matrix(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.).finished();
SharedDiagonal expected = noiseModel::Constrained::All(2);
Matrix expectedAb = (Matrix(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).finished();
SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expected,*actual));
@ -317,7 +317,7 @@ TEST(NoiseModel, ScalarOrVector )
/* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace)
{
Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1);
Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1).finished();
SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3);
model->WhitenInPlace(A);
@ -340,8 +340,8 @@ TEST(NoiseModel, robustFunction)
TEST(NoiseModel, robustNoise)
{
const double k = 10.0, error1 = 1.0, error2 = 100.0;
Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0);
Vector b = (Vector(2) << error1, error2);
Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
Vector b = (Vector(2) << error1, error2).finished();
const Robust::shared_ptr robust = Robust::Create(
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
Unit::Create(2));

2
gtsam/linear/tests/testSampler.cpp
View File

@ -24,7 +24,7 @@ const double tol = 1e-5;
/* ************************************************************************* */
TEST(testSampler, basic) {
Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0);
Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0).finished();
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);

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((Vector(3) << 0.1, 0.2, 0.3));
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished());
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((Vector(3) << 0.0, 0.0, 0.1));
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vector(3) << 0.0, 0.0, 0.1).finished());
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, (Vector(1) << 1.0));
values.insert(1, (Vector(2) << 2.0,3.0));
values.insert(2, (Vector(2) << 4.0,5.0));
values.insert(0, (Vector(1) << 1.0).finished());
values.insert(1, (Vector(2) << 2.0,3.0).finished());
values.insert(2, (Vector(2) << 4.0,5.0).finished());
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((Vector(3) << 1.0, 2.0, 3.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0).finished());
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 = (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);
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
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 = (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);
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
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((Vector(3) << 1.0, 2.0, 3.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0).finished());
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, (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));
(JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);

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

@ -36,10 +36,10 @@ TEST(VectorValues, basics)
// insert
VectorValues actual;
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));
actual.insert(0, (Vector(1) << 1).finished());
actual.insert(1, (Vector(2) << 2, 3).finished());
actual.insert(5, (Vector(2) << 6, 7).finished());
actual.insert(2, (Vector(2) << 4, 5).finished());
// Check dimensions
LONGS_EQUAL(4, actual.size());
@ -58,12 +58,12 @@ TEST(VectorValues, basics)
EXPECT(!actual.exists(6));
// Check values
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]));
EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0]));
EXPECT(assert_equal((Vector(2) << 2, 3).finished(), actual[1]));
EXPECT(assert_equal((Vector(2) << 4, 5).finished(), actual[2]));
EXPECT(assert_equal((Vector(2) << 6, 7).finished(), actual[5]));
FastVector<Key> keys = list_of(0)(1)(2)(5);
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), 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, (Vector(1) << 1));
expected.insert(1, (Vector(2) << 2, 3));
expected.insert(5, (Vector(2) << 6, 7));
expected.insert(2, (Vector(2) << 4, 5));
expected.insert(0, (Vector(1) << 1).finished());
expected.insert(1, (Vector(2) << 2, 3).finished());
expected.insert(5, (Vector(2) << 6, 7).finished());
expected.insert(2, (Vector(2) << 4, 5).finished());
VectorValues first;
first.insert(0, (Vector(1) << 1));
first.insert(1, (Vector(2) << 2, 3));
first.insert(0, (Vector(1) << 1).finished());
first.insert(1, (Vector(2) << 2, 3).finished());
VectorValues second;
second.insert(5, (Vector(2) << 6, 7));
second.insert(2, (Vector(2) << 4, 5));
second.insert(5, (Vector(2) << 6, 7).finished());
second.insert(2, (Vector(2) << 4, 5).finished());
VectorValues actual(first, second);
@ -96,14 +96,14 @@ TEST(VectorValues, combine)
TEST(VectorValues, subvector)
{
VectorValues init;
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));
init.insert(10, (Vector(1) << 1).finished());
init.insert(11, (Vector(2) << 2, 3).finished());
init.insert(12, (Vector(2) << 4, 5).finished());
init.insert(13, (Vector(2) << 6, 7).finished());
std::vector<Key> keys;
keys += 10, 12, 13;
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7);
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
EXPECT(assert_equal(expSubVector, init.vector(keys)));
}
@ -111,16 +111,16 @@ TEST(VectorValues, subvector)
TEST(VectorValues, LinearAlgebra)
{
VectorValues test1;
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));
test1.insert(0, (Vector(1) << 1).finished());
test1.insert(1, (Vector(2) << 2, 3).finished());
test1.insert(5, (Vector(2) << 6, 7).finished());
test1.insert(2, (Vector(2) << 4, 5).finished());
VectorValues test2;
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));
test2.insert(0, (Vector(1) << 6).finished());
test2.insert(1, (Vector(2) << 1, 6).finished());
test2.insert(5, (Vector(2) << 4, 3).finished());
test2.insert(2, (Vector(2) << 1, 8).finished());
// 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, (Vector(1) << 1));
expected.insert(1, (Vector(2) << 2, 3));
expected.insert(2, (Vector(2) << 4, 5));
expected.insert(5, (Vector(2) << 6, 7));
expected.insert(0, (Vector(1) << 1).finished());
expected.insert(1, (Vector(2) << 2, 3).finished());
expected.insert(2, (Vector(2) << 4, 5).finished());
expected.insert(5, (Vector(2) << 6, 7).finished());
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, (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));
vv.insert(0, (Vector(1) << 1).finished());
vv.insert(1, (Vector(2) << 2, 3).finished());
vv.insert(2, (Vector(2) << 4, 5).finished());
vv.insert(5, (Vector(2) << 6, 7).finished());
vv.insert(7, (Vector(2) << 8, 9).finished());
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((Vector(3) << 0.1,0.2,0.4));
Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03));
Vector bias_acc((Vector(3) << 0.1,0.2,0.4).finished());
Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03).finished());
// Default Constructor
gtsam::imuBias::ConstantBias bias1;

20
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished());
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished());
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -239,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished());
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished());
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished());
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished());
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -446,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{
// // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished());
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished());
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator
@ -502,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished());
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished());
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

4
gtsam/nonlinear/ISAM2.h
View File

@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0).finished(); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/

12
gtsam/nonlinear/WhiteNoiseFactor.h
View File

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

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

@ -19,7 +19,7 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0));
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0).finished());
const double tol = 1e-5;
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
@ -32,12 +32,12 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
0.0, 2.63624);
0.0, 2.63624).finished();
Matrix A2 = (Matrix(2, 2) <<
-0.0455167, -0.0443573,
-0.0222154, -0.102489);
-0.0222154, -0.102489).finished();
Vector b = (Vector(2) << 0.0277052,
-0.0533393);
-0.0533393).finished();
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
@ -66,12 +66,12 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
0.0, 2.63624);
0.0, 2.63624).finished();
Matrix A2 = (Matrix(2, 2) <<
-0.0455167, -0.0443573,
-0.0222154, -0.102489);
-0.0222154, -0.102489).finished();
Vector b = (Vector(2) << 0.0277052,
-0.0533393);
-0.0533393).finished();
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 = (Vector(2) << 1.0, 2.0);
Vector delta_l2 = (Vector(2) << 3.0, 4.0);
Vector delta_l1 = (Vector(2) << 1.0, 2.0).finished();
Vector delta_l2 = (Vector(2) << 3.0, 4.0).finished();
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 = (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 G11 = (Matrix(1, 1) << 1.0).finished();
Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
Matrix G22 = (Matrix(2, 2) << 3.0, 5.0,
0.0, 6.0);
0.0, 6.0).finished();
Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0,
1.0, 2.0, 4.0);
1.0, 2.0, 4.0).finished();
Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0,
0.0, 5.0, 6.0,
0.0, 0.0, 9.0);
0.0, 0.0, 9.0).finished();
Vector g1 = (Vector(1) << -7.0);
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0);
Vector g1 = (Vector(1) << -7.0).finished();
Vector g2 = (Vector(2) << -8.0, -9.0).finished();
Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished();
double f = 10.0;
@ -161,18 +161,18 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Matrix G11 = (Matrix(3, 3) <<
1.0, 2.0, 3.0,
0.0, 5.0, 6.0,
0.0, 0.0, 9.0);
0.0, 0.0, 9.0).finished();
Matrix G12 = (Matrix(3, 2) <<
1.0, 2.0,
3.0, 5.0,
4.0, 6.0);
Vector g1 = (Vector(3) << 1.0, 2.0, 3.0);
4.0, 6.0).finished();
Vector g1 = (Vector(3) << 1.0, 2.0, 3.0).finished();
Matrix G22 = (Matrix(2, 2) <<
0.5, 0.2,
0.0, 0.6);
0.0, 0.6).finished();
Vector g2 = (Vector(2) << -8.0, -9.0);
Vector g2 = (Vector(2) << -8.0, -9.0).finished();
double f = 10.0;
@ -197,16 +197,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
EXPECT(assert_equal(expLinPoints, actLinPoint));
// Create delta
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);
Vector delta_l1 = (Vector(2) << 1.0, 2.0).finished();
Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5).finished();
Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3).finished();
// 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((Vector(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).finished(), 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 = (Vector(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).finished();
Vector g(5); g << g1, g2;
Vector g_prime = g - G.selfadjointView<Eigen::Upper>() * dv;

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

@ -194,7 +194,7 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero)
//{
// Values config0;
// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0));
// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0).finished());
// config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim());
//
@ -212,8 +212,8 @@ TEST(Values, expmap_a)
config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5));
(key1, (Vector(3) << 1.0, 1.1, 1.2).finished())
(key2, (Vector(3) << 1.3, 1.4, 1.5).finished());
Values expected;
expected.insert(key1, Vector3(2.0, 3.1, 4.2));
@ -230,7 +230,7 @@ TEST(Values, expmap_b)
config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5));
(key2, (Vector(3) << 1.3, 1.4, 1.5).finished());
Values expected;
expected.insert(key1, Vector3(1.0, 2.0, 3.0));
@ -283,8 +283,8 @@ TEST(Values, localCoordinates)
valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3))
(key2, (Vector(3) << 0.4, 0.5, 0.6));
(key1, (Vector(3) << 0.1, 0.2, 0.3).finished())
(key2, (Vector(3) << 0.4, 0.5, 0.6).finished());
Values valuesB = valuesA.retract(expDelta);

2
gtsam/slam/BearingRangeFactor.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = (Vector(1) << y2 - measuredRange_);
Vector e2 = (Vector(1) << y2 - measuredRange_).finished();
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);

8
gtsam/slam/BoundingConstraint.h
View File

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

4
gtsam/slam/InitializePose3.cpp
View File

@ -65,7 +65,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
linearGraph.add(key1, -I9, key2, M9, zero9, model);
}
// prior on the anchor orientation
linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model);
linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model);
return linearGraph;
}
@ -291,7 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
double th = logRot.norm();
if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished()) ); // some perturbation
logRot = Rot3::Logmap(R1pert.between(R2));
th = logRot.norm();
}

2
gtsam/slam/RangeFactor.h
View File

@ -73,7 +73,7 @@ namespace gtsam {
} else {
hx = pose.range(point, H1, H2);
}
return (Vector(1) << hx - measured_);
return (Vector(1) << hx - measured_).finished();
}
/** return the measured */

2
gtsam/slam/RotateFactor.h
View File

@ -55,7 +55,7 @@ public:
// predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3
return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z();
return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
}
};

2
gtsam/slam/dataset.cpp
View File

@ -313,7 +313,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
// Create noise model
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished());
// Add to graph
*graph += BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range,

18
gtsam/slam/lago.cpp
View File

@ -35,9 +35,9 @@ static const Matrix I3 = eye(3);
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Sigmas((Vector(1) << 0));
noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished());
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8).finished());
/* ************************************************************************* */
/**
@ -143,7 +143,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
if (!pose2Between)
throw invalid_argument(
"buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta());
deltaTheta = (Vector(1) << pose2Between->measured().theta()).finished();
// Retrieve the noise model for the relative rotation
SharedNoiseModel model = pose2Between->get_noiseModel();
@ -152,7 +152,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
if (!diagonalModel)
throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal noise model)!");
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)).finished(); // std on the angular measurement
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
}
@ -185,11 +185,11 @@ GaussianFactorGraph buildLinearOrientationGraph(
double k = boost::math::round(k2pi_noise / (2 * M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
Vector deltaThetaRegularized = (Vector(1)
<< key1_DeltaTheta_key2 - 2 * k * M_PI);
<< key1_DeltaTheta_key2 - 2 * k * M_PI).finished();
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
}
// prior on the anchor orientation
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
return lagoGraph;
}
@ -321,8 +321,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
double dy = pose2Between->measured().y();
Vector globalDeltaCart = //
(Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy);
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
(Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy).finished();
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot).finished(); // rhs
Matrix J1 = -I3;
J1(0, 2) = s1 * dx + c1 * dy;
J1(1, 2) = -c1 * dx + s1 * dy;
@ -338,7 +338,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
}
}
// add prior
linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0).finished(),
priorPose2Noise);
// optimize

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

@ -110,7 +110,7 @@ TEST( dataSet, readG2o)
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished());
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
@ -159,7 +159,7 @@ TEST( dataSet, readG2o3D)
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
@ -238,7 +238,7 @@ TEST( dataSet, readG2oHuber)
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished());
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
NonlinearFactorGraph expectedGraph;
@ -266,7 +266,7 @@ TEST( dataSet, readG2oTukey)
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished());
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
NonlinearFactorGraph expectedGraph;

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

@ -71,7 +71,7 @@ TEST (EssentialMatrixFactor, testData) {
EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
// Check homogeneous version
EXPECT(assert_equal((Vector(3) << -1, 0.2, 1), vB(4), 1e-8));
EXPECT(assert_equal((Vector(3) << -1, 0.2, 1).finished(), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
@ -126,7 +126,7 @@ TEST (EssentialMatrixFactor, minimization) {
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
@ -341,7 +341,7 @@ TEST (EssentialMatrixFactor, extraMinimization) {
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)

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 = (Vector(2) << 323.,240.);
Vector z = (Vector(2) << 323.,240.).finished();
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(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values)));
EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0).finished()), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;
@ -315,7 +315,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
focal_noise, focal_noise, // f_x, f_y
skew_noise, // s
trans_noise, trans_noise // ux, uy
) ;
).finished();
values.insert(X(i), cameras[i].retract(delta)) ;
}
}

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 = (Vector(2) << 323.,240.);
Vector z = (Vector(2) << 323.,240.).finished();
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((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal((Vector(2) << -3.0, 0.0).finished(), factor->unwhitenedError(values)));
}
@ -312,7 +312,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
) ;
).finished();
values.insert(X(i), cameras[i].retract(delta)) ;
}
}

20
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -47,13 +47,13 @@ namespace simple {
// x0
//
static Point3 p0 = Point3(0,0,0);
static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) );
static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ).finished() );
static Point3 p1 = Point3(1,2,0);
static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) );
static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ).finished() );
static Point3 p2 = Point3(0,2,0);
static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) );
static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ).finished() );
static Point3 p3 = Point3(-1,1,0);
static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) );
static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ).finished() );
static Pose3 pose0 = Pose3(R0,p0);
static Pose3 pose1 = Pose3(R1,p1);
@ -145,7 +145,7 @@ TEST( InitializePose3, iterationGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished());
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
@ -158,25 +158,25 @@ TEST( InitializePose3, iterationGradient ) {
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
0.033572116359134, 0.999436104312325, -0.000621610948719,
-0.000983333645009, 0.000654992453817, 0.999999302019670);
-0.000983333645009, 0.000654992453817, 0.999999302019670).finished();
Rot3 R0Expected = Rot3(M0);
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114,
0.010943459008004, 0.999898317528125, -0.009143047050380,
-0.008336465609239, 0.009234508232789, 0.999922610604863);
-0.008336465609239, 0.009234508232789, 0.999922610604863).finished();
Rot3 R1Expected = Rot3(M1);
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-5));
Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553,
-0.045306446926148, 0.998936408933058, 0.008566024448664,
0.008538487960253, -0.008187284445083, 0.999930028850403);
0.008538487960253, -0.008187284445083, 0.999930028850403).finished();
Rot3 R2Expected = Rot3(M2);
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-5));
Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275,
0.010911315499947, 0.999906044037258, -0.008297366559388,
-0.009132272433995, 0.008397162077148, 0.999923041673329);
-0.009132272433995, 0.008397162077148, 0.999923041673329).finished();
Rot3 R3Expected = Rot3(M3);
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-5));
}
@ -186,7 +186,7 @@ TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished());
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));

56
gtsam/slam/tests/testLago.cpp
View File

@ -129,11 +129,11 @@ TEST( Lago, regularizedMeasurements ) {
GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)).finished();
// this is the whitened error, so we multiply by the std to unwhiten
actual = 0.1 * actual;
// Expected regularized measurements (same for the spanning tree, corrected for the chordsIds)
Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2);
Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2).finished();
EXPECT(assert_equal(expected, actual, 1e-6));
}
@ -144,10 +144,10 @@ TEST( Lago, smallGraphVectorValues ) {
VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -156,10 +156,10 @@ TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initial = lago::initializeOrientations(simpleLago::graph());
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -170,10 +170,10 @@ TEST( Lago, multiplePosePriors ) {
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -183,10 +183,10 @@ TEST( Lago, multiplePosePriorsSP ) {
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -197,10 +197,10 @@ TEST( Lago, multiplePoseAndRotPriors ) {
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -210,10 +210,10 @@ TEST( Lago, multiplePoseAndRotPriorsSP ) {
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -265,7 +265,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4).finished());
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
@ -300,7 +300,7 @@ TEST( Lago, largeGraphNoisy ) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4).finished());
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = lago::initialize(graphWithPrior);

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

@ -18,8 +18,8 @@
using namespace gtsam;
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished());
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished());
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((Vector(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).finished());
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
@ -63,9 +63,9 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3).finished(), factor.evaluateError(pose1, actH1)));
#else
EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3).finished(), factor.evaluateError(pose1, actH1),1e-2));
#endif
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
// the derivative is more complex, but is close to the identity for Rot3 around the origin
@ -88,7 +88,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -98,7 +98,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2 pose1(rot2C, point2A);
Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, 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((Vector(2) << 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2).finished());
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished());
typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior;
typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior;
@ -58,7 +58,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -78,7 +78,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3 pose1(rot3B, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -98,7 +98,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3 pose1(rot3C, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -118,7 +118,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(2) << -3.0,-4.0).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

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