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

Made 'between' derivatives in the tangent space of the solution instead of tangent space of identity, this makes Pose2 an "origin-free" manifold.

release/4.3a0
Richard Roberts 2009-12-21 16:43:23 +00:00
parent d0b757da48
commit 92b60a8543
6 changed files with 204 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
cpp/Pose2.cpp
View File

@ -19,20 +19,6 @@ namespace gtsam {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
} }
/* ************************************************************************* */
Pose2 Pose2::exmap(const Vector& v) const {
return Pose2(r_.exmap(Vector_(1, v(2))), t_+Point2(v(0),v(1)));
}
/* ************************************************************************* */
Vector Pose2::log(const Pose2 &pose) const {
return Vector_(3,
pose.t().x() - t().x(),
pose.t().y() - t().y(),
pose.r().theta() - r().theta());
// return between(*this, pose).vector();
}
/* ************************************************************************* */ /* ************************************************************************* */
// Pose2 Pose2::rotate(double theta) const { // Pose2 Pose2::rotate(double theta) const {
// //return Pose2(t_, Rot2(theta)*r_); // //return Pose2(t_, Rot2(theta)*r_);
@ -46,7 +32,7 @@ namespace gtsam {
// TODO, have a combined function that returns both function value and derivative // TODO, have a combined function that returns both function value and derivative
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
Matrix dx_dt = pose.r().negtranspose(); Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0);
Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t()); Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
return collect(2, &dx_dt, &dx_dr); return collect(2, &dx_dt, &dx_dr);
} }
@ -63,17 +49,21 @@ namespace gtsam {
} }
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
Matrix dbt_dp = Dtransform_to1(p1, p2.t()); Matrix dt_dr = Dunrotate1(p1.r(), p2.t()-p1.t());
Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0); Matrix dt_dt1 = -p2.r().invcompose(p1.r()).matrix();
return stack(2, &dbt_dp, &dbr_dp); Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p1.t());
return Matrix_(3,3,
dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0),
dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0),
0.0, 0.0, -1.0);
} }
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
Matrix db_dt2 = p1.r().transpose(); Matrix db_dt2 = p1.r().transpose();
return Matrix_(3,3, return Matrix_(3,3,
db_dt2.data()[0], db_dt2.data()[1], 0.0, 1.0, 0.0, 0.0,
db_dt2.data()[2], db_dt2.data()[3], 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
} }
/* ************************************************************************* */ /* ************************************************************************* */

49
cpp/Pose2.h
View File

@ -16,6 +16,21 @@
namespace gtsam { namespace gtsam {
/**
* Return point coordinates in pose coordinate frame
*/
class Pose2;
Point2 transform_to(const Pose2& pose, const Point2& point);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2);
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
/** /**
* A 2D pose (Point2,Rot2) * A 2D pose (Point2,Rot2)
*/ */
@ -70,48 +85,26 @@ namespace gtsam {
size_t dim() const { return 3; } size_t dim() const { return 3; }
/** exponential map */ /** exponential map */
Pose2 exmap(const Vector& v) const; Pose2 exmap(const Vector& v) const { return Pose2(v[0], v[1], v[2]) * (*this); }
/** log map */ /** log map */
Vector log(const Pose2 &pose) const; Vector log(const Pose2 &pose) const { return between(*this, pose).vector(); }
/** rotate pose by theta */ /** rotate pose by theta */
// Pose2 rotate(double theta) const; // Pose2 rotate(double theta) const;
/** inverse transformation */ /** inverse transformation */
Pose2 inverse() const { Pose2 inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); }
return Pose2(r_, r_.unrotate(t_));
}
/** compose this transformation onto another (pre-multiply this*p1) */ /** compose this transformation onto another (pre-multiply this*p1) */
Pose2 compose(const Pose2& p1) const { Pose2 compose(const Pose2& p1) const { return Pose2(p1.r_ * r_, p1.r_ * t_ + p1.t_); }
return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
}
/** same as compose (pre-multiply this*p1) */ /** same as compose (pre-multiply this*p1) */
Pose2 operator*(const Pose2& p1) const { Pose2 operator*(const Pose2& p1) const { return compose(p1); }
return compose(p1);
}
/** Return point coordinates in pose coordinate frame, same as transform_to */ /** Return point coordinates in pose coordinate frame, same as transform_to */
Point2 operator*(const Point2& point) const { Point2 operator*(const Point2& point) const { return r_.unrotate(point-t_); }
return r_.unrotate(point-t_);
}
}; // Pose2 }; // Pose2
/**
* Return point coordinates in pose coordinate frame
*/
Point2 transform_to(const Pose2& pose, const Point2& point);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2);
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
} // namespace gtsam } // namespace gtsam

83
cpp/Pose2Config.h
View File

@ -18,44 +18,46 @@
namespace gtsam { namespace gtsam {
class Pose2Config: public Testable<Pose2Config> { class Pose2Config: public Testable<Pose2Config> {
private: private:
std::map<std::string, Pose2> values_; std::map<std::string, Pose2> values_;
public: public:
typedef std::map<std::string, Pose2>::const_iterator iterator;
typedef iterator const_iterator;
Pose2Config() {} Pose2Config() {}
Pose2Config(const Pose2Config &config) : values_(config.values_) { } Pose2Config(const Pose2Config &config) : values_(config.values_) { }
virtual ~Pose2Config() { } virtual ~Pose2Config() { }
Pose2 get(std::string key) const { Pose2 get(std::string key) const {
std::map<std::string, Pose2>::const_iterator it = values_.find(key); std::map<std::string, Pose2>::const_iterator it = values_.find(key);
if (it == values_.end()) if (it == values_.end())
throw std::invalid_argument("invalid key"); throw std::invalid_argument("invalid key");
return it->second; return it->second;
} }
void insert(const std::string& name, const Pose2& val){ void insert(const std::string& name, const Pose2& val){
values_.insert(make_pair(name, val)); values_.insert(make_pair(name, val));
} }
Pose2Config& operator=(Pose2Config& rhs) { Pose2Config& operator=(Pose2Config& rhs) {
values_ = rhs.values_; values_ = rhs.values_;
return (*this); return (*this);
} }
bool equals(const Pose2Config& expected, double tol) const { bool equals(const Pose2Config& expected, double tol) const {
std::cerr << "Pose2Config::equals not implemented!" << std::endl; std::cerr << "Pose2Config::equals not implemented!" << std::endl;
throw "Pose2Config::equals not implemented!"; throw "Pose2Config::equals not implemented!";
} }
void print(const std::string &s) const { void print(const std::string &s) const {
std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n"; std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n";
std::string j; Pose2 v; std::string j; Pose2 v;
FOREACH_PAIR(j, v, values_) { FOREACH_PAIR(j, v, values_) {
v.print(j + ": "); v.print(j + ": ");
} }
} }
/** /**
* Add a delta config, needed for use in NonlinearOptimizer * Add a delta config, needed for use in NonlinearOptimizer
@ -64,16 +66,19 @@ public:
Pose2Config newConfig; Pose2Config newConfig;
std::string j; Pose2 vj; std::string j; Pose2 vj;
FOREACH_PAIR(j, vj, values_) { FOREACH_PAIR(j, vj, values_) {
if (delta.contains(j)) { if (delta.contains(j)) {
const Vector& dj = delta[j]; const Vector& dj = delta[j];
//check_size(j,vj,dj); //check_size(j,vj,dj);
newConfig.insert(j, vj.exmap(dj)); newConfig.insert(j, vj.exmap(dj));
} else { } else {
newConfig.insert(j, vj); newConfig.insert(j, vj);
} }
} }
return newConfig; return newConfig;
} }
}; iterator begin() const { return values_.begin(); }
iterator end() const { return values_.end(); }
};
} // namespace } // namespace

95
cpp/testPose2.cpp
View File

@ -5,12 +5,33 @@
#include <math.h> #include <math.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream>
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "Pose2.h" #include "Pose2.h"
#include "Point2.h" #include "Point2.h"
#include "Rot2.h" #include "Rot2.h"
using namespace gtsam; using namespace gtsam;
using namespace std;
Matrix toManifoldDerivative(Matrix vectorDerivative, Pose2 linearizationPoint) {
Matrix manifoldDerivative(vectorDerivative.size1(), vectorDerivative.size2());
for(int j=0; j<vectorDerivative.size2(); j++) {
// Find the pose implied by the vector derivative, which is usually
// linearized about identity.
Pose2 partialPose = Pose2().exmap(Vector_(3,
vectorDerivative(0,j),
vectorDerivative(1,j),
vectorDerivative(2,j)) + linearizationPoint.vector());
// Now convert the derivative to the tangent space of the new linearization
// point.
Vector manifoldPartial = linearizationPoint.log(partialPose);
manifoldDerivative(0,j) = manifoldPartial(0);
manifoldDerivative(1,j) = manifoldPartial(1);
manifoldDerivative(2,j) = manifoldPartial(2);
}
return manifoldDerivative;
}
/* ************************************************************************* */ /* ************************************************************************* */
//TEST(Pose2, print) { //TEST(Pose2, print) {
@ -28,17 +49,25 @@ TEST(Pose2, constructors) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, exmap) { TEST(Pose2, exmap) {
Pose2 pose(M_PI_2, Point2(1,2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(M_PI_2+0.018, Point2(1.01, 1.985)); Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */
TEST(Pose2, exmap0) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 actual = Pose2().exmap(Vector_(3, 0.01, -0.015, 0.018)) * pose;
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, log) { TEST(Pose2, log) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI, Point2(1.1, 2.1)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.1, 0.1, M_PI_2); Vector expected = Vector_(3, 0.01, -0.015, 0.018);
Vector actual = pose0.log(pose); Vector actual = pose0.log(pose);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
@ -52,12 +81,6 @@ TEST(Pose2, log) {
// CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta))); // CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
//} //}
/* ************************************************************************* */
//TEST(Pose2, operators) {
// CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
// CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, transform_to ) TEST( Pose2, transform_to )
{ {
@ -66,8 +89,8 @@ TEST( Pose2, transform_to )
// expected // expected
Point2 expected(2,2); Point2 expected(2,2);
Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0); // Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); // Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
// actual // actual
Point2 actual = transform_to(pose,point); Point2 actual = transform_to(pose,point);
@ -75,8 +98,8 @@ TEST( Pose2, transform_to )
Matrix actualH2 = Dtransform_to2(pose,point); Matrix actualH2 = Dtransform_to2(pose,point);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1)); // CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2)); // CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5); Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
@ -87,6 +110,26 @@ TEST( Pose2, transform_to )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, compose_a) TEST(Pose2, compose_a)
{
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
Pose2 actual = pose2 * pose1;
CHECK(assert_equal(expected, actual));
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = pose2 * pose1 * point;
Point2 actual_point2 = (pose2 * pose1) * point;
Point2 actual_point3 = pose2 * (pose1 * point);
CHECK(assert_equal(expected_point, actual_point1));
CHECK(assert_equal(expected_point, actual_point2));
CHECK(assert_equal(expected_point, actual_point3));
}
/* ************************************************************************* */
TEST(Pose2, compose_b)
{ {
Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5)); Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5));
Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585)); Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
@ -101,7 +144,7 @@ TEST(Pose2, compose_a)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, compose_b) TEST(Pose2, compose_c)
{ {
Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0)); Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0));
Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5))); Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
@ -125,14 +168,14 @@ TEST( Pose2, between )
// expected // expected
Pose2 expected(M_PI_2, Point2(2,2)); Pose2 expected(M_PI_2, Point2(2,2));
Matrix expectedH1 = Matrix_(3,3, Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,2.0, 0.0,-1.0,-2.0,
1.0,0.0,-2.0, 1.0, 0.0,-2.0,
0.0,0.0,-1.0 0.0, 0.0,-1.0
); );
Matrix expectedH2 = Matrix_(3,3, Matrix expectedH2 = Matrix_(3,3,
0.0,1.0,0.0, 1.0, 0.0, 0.0,
-1.0,0.0,0.0, 0.0, 1.0, 0.0,
0.0,0.0,1.0 0.0, 0.0, 1.0
); );
// actual // actual
@ -140,14 +183,16 @@ TEST( Pose2, between )
Matrix actualH1 = Dbetween1(p1,p2); Matrix actualH1 = Dbetween1(p1,p2);
Matrix actualH2 = Dbetween2(p1,p2); Matrix actualH2 = Dbetween2(p1,p2);
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
numericalH1 = toManifoldDerivative(numericalH1, between(p1,p2));
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
numericalH2 = toManifoldDerivative(numericalH2, between(p1,p2));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2)); CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }

67
cpp/testPose2Factor.cpp
View File

@ -8,12 +8,15 @@
#include <iostream> #include <iostream>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <Boost/shared_ptr.hpp>
#include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
#include "Pose2Factor.h" #include "Pose2Factor.h"
#include "Pose2Graph.h" #include "Pose2Graph.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost;
TEST( Pose2Factor, constructor ) TEST( Pose2Factor, constructor )
{ {
@ -32,38 +35,68 @@ TEST( Pose2Factor, constructor )
Pose2Config config; Pose2Config config;
config.insert("p1",p1); config.insert("p1",p1);
config.insert("p2",p2); config.insert("p2",p2);
//Pose2 pose1;
//pose1=config.get("p1");
//pose1.print("pose1");
// Linearize // Linearize
boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config); boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
// expected // expected
Matrix expectedH1 = Matrix_(3,3, Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,2.1, 0.0,-1.0,-2.1,
1.0,0.0,-2.1, 1.0, 0.0,-2.1,
0.0,0.0,-1.0 0.0, 0.0,-1.0
); );
Matrix expectedH2 = Matrix_(3,3, Matrix expectedH2 = Matrix_(3,3,
0.0,1.0,0.0, 1.0, 0.0, 0.0,
-1.0,0.0,0.0, 0.0, 1.0, 0.0,
0.0,0.0,1.0 0.0, 0.0, 1.0
); );
// we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!) // we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!)
Matrix square_root_inverse_covariance = Matrix_(3,3, Matrix square_root_inverse_covariance = Matrix_(3,3,
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -10.0 0.0, 0.0, -10.0
); );
GaussianFactor expected( GaussianFactor expected(
"p1", square_root_inverse_covariance*expectedH1, "p1", square_root_inverse_covariance*expectedH1,
"p2", square_root_inverse_covariance*expectedH2, "p2", square_root_inverse_covariance*expectedH2,
Vector_(3,0.1,0.1,0.0), 1.0); Vector_(3, 0.1, -0.1, 0.0), 1.0);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
} }
bool poseCompare(const std::string& key,
const gtsam::Pose2Config& feasible,
const gtsam::Pose2Config& input) {
return feasible.get(key).equals(input.get(key));
}
TEST(Pose2Factor, optimize) {
Pose2Graph fg;
shared_ptr<Pose2Config> config = shared_ptr<Pose2Config>(new Pose2Config());
Pose2Config feasible;
feasible.insert("p0", Pose2(0,0,0));
fg.push_back(Pose2Graph::sharedFactor(
new NonlinearEquality<Pose2Config>("p0", feasible, Pose2().dim(), poseCompare)));
config->insert("p0", Pose2(0,0,0));
fg.push_back(Pose2Graph::sharedFactor(
new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3,
0.5, 0.0, 0.0,
0.0, 0.5, 0.0,
0.0, 0.0, 0.5))));
config->insert("p1", Pose2(0,0,0));
Ordering ordering;
ordering.push_back("p0");
ordering.push_back("p1");
NonlinearOptimizer<Pose2Graph, Pose2Config> optimizer(fg, ordering, config);
optimizer = optimizer.levenbergMarquardt(1e-10, 1e-10);
Pose2 actual0 = optimizer.config()->get("p0");
Pose2 actual1 = optimizer.config()->get("p1");
Pose2 expected0 = Pose2(0,0,0);
Pose2 expected1 = Pose2(1,2,M_PI_2);
CHECK(assert_equal(expected0, actual0));
CHECK(assert_equal(expected0, actual0));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {

16
cpp/testPose2Graph.cpp
View File

@ -56,22 +56,22 @@ TEST( Pose2Graph, linerization )
// the expected linear factor // the expected linear factor
GaussianFactorGraph lfg_expected; GaussianFactorGraph lfg_expected;
Matrix A1 = Matrix_(3,3, Matrix A1 = Matrix_(3,3,
0.0, 2.0, -4.2, 0.0, 2.0, 4.2,
-2.0, 0.0, 4.2, -2.0, 0.0, 4.2,
0.0, 0.0, 10.0); 0.0, 0.0, 10.0);
Matrix A2 = Matrix_(3,3, Matrix A2 = Matrix_(3,3,
0.0, -2.0, 0.0, -2.0, 0.0, 0.0,
2.0, 0.0, 0.0, 0.0,-2.0, 0.0,
0.0, 0.0, -10.0); 0.0, 0.0, -10.0);
double sigma = 1; double sigma = 1;
Vector b = Vector_(3,0.1,0.1,0.0); Vector b = Vector_(3,0.1,-0.1,0.0);
lfg_expected.add("x1", A1, "x2", A2, b, sigma); lfg_expected.add("x1", A1, "x2", A2, b, sigma);
CHECK(lfg_expected.equals(lfg_linearized)); CHECK(assert_equal(lfg_expected, lfg_linearized));
} }
/* ************************************************************************* */ /* ************************************************************************* */