Frank Dellaert
commit
b2dcf35e77
|
|
@ -532,6 +532,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSimilarity3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
/org.eclipse.cdt.codan.core.prefs
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Similarity3.cpp
|
||||
* @brief Implementation of Similarity3 transform
|
||||
* @author Paul Drews
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
|
||||
R_(R), t_(t), s_(s) {
|
||||
}
|
||||
|
||||
Similarity3::Similarity3() :
|
||||
R_(), t_(), s_(1){
|
||||
}
|
||||
|
||||
Similarity3::Similarity3(double s) :
|
||||
s_ (s) {
|
||||
}
|
||||
|
||||
Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
|
||||
R_ (R), t_ (t), s_ (s) {
|
||||
}
|
||||
|
||||
Similarity3::operator Pose3() const {
|
||||
return Pose3(R_, s_*t_);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::identity() {
|
||||
return Similarity3(); }
|
||||
|
||||
//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
||||
// return Vector7();
|
||||
//}
|
||||
//
|
||||
//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
|
||||
// return Similarity3();
|
||||
//}
|
||||
|
||||
bool Similarity3::operator==(const Similarity3& other) const {
|
||||
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
|
||||
}
|
||||
|
||||
bool Similarity3::equals(const Similarity3& sim, double tol) const {
|
||||
return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
|
||||
&& s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
|
||||
}
|
||||
|
||||
Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
|
||||
return R_ * (s_ * p) + t_;
|
||||
}
|
||||
|
||||
Matrix7 Similarity3::AdjointMap() const{
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = s_ * skewSymmetric(t) * R;
|
||||
Matrix7 adj;
|
||||
adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 1, 6>::Zero(), 1;
|
||||
return adj;
|
||||
}
|
||||
|
||||
inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
|
||||
return transform_from(p);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::inverse() const {
|
||||
Rotation Rt = R_.inverse();
|
||||
Translation sRt = R_.inverse() * (-s_ * t_);
|
||||
return Similarity3(Rt, sRt, 1.0/s_);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::operator*(const Similarity3& T) const {
|
||||
return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
|
||||
}
|
||||
|
||||
void Similarity3::print(const std::string& s) const {
|
||||
std::cout << std::endl;
|
||||
std::cout << s;
|
||||
rotation().print("R:\n");
|
||||
translation().print("t: ");
|
||||
std::cout << "s: " << scale() << std::endl;
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) {
|
||||
// Will retracting or localCoordinating R work if R is not a unit rotation?
|
||||
// Also, how do we actually get s out? Seems like we need to store it somewhere.
|
||||
Rotation r; //Create a zero rotation to do our retraction.
|
||||
return Similarity3( //
|
||||
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
|
||||
Translation(v.segment<3>(3)), // Retract the translation
|
||||
1.0 + v[6]); //finally, update scale using v[6]
|
||||
}
|
||||
|
||||
Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) {
|
||||
Rotation r; //Create a zero rotation to do the retraction
|
||||
Vector7 v;
|
||||
v.head<3>() = r.localCoordinates(other.R_);
|
||||
v.segment<3>(3) = other.t_.vector();
|
||||
//v.segment<3>(3) = translation().localCoordinates(other.translation());
|
||||
v[6] = other.s_ - 1.0;
|
||||
return v;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Similarity3.h
|
||||
* @brief Implementation of Similarity3 transform
|
||||
* @author Paul Drews
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class Pose3;
|
||||
|
||||
/**
|
||||
* 3D similarity transform
|
||||
*/
|
||||
class Similarity3: public LieGroup<Similarity3, 7> {
|
||||
|
||||
/** Pose Concept requirements */
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
|
||||
private:
|
||||
Rotation R_;
|
||||
Translation t_;
|
||||
double s_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
Similarity3();
|
||||
|
||||
/// Construct pure scaling
|
||||
Similarity3(double s);
|
||||
|
||||
/// Construct from GTSAM types
|
||||
Similarity3(const Rotation& R, const Translation& t, double s);
|
||||
|
||||
/// Construct from Eigen types
|
||||
Similarity3(const Matrix3& R, const Vector3& t, double s);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Compare with tolerance
|
||||
bool equals(const Similarity3& sim, double tol) const;
|
||||
|
||||
/// Compare with standard tolerance
|
||||
bool operator==(const Similarity3& other) const;
|
||||
|
||||
/// Print with optional string
|
||||
void print(const std::string& s) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// Return an identity transform
|
||||
static Similarity3 identity();
|
||||
|
||||
/// Return the inverse
|
||||
Similarity3 inverse() const;
|
||||
|
||||
Translation transform_from(const Translation& p) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Translation operator*(const Translation& p) const;
|
||||
|
||||
Similarity3 operator*(const Similarity3& T) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard interface
|
||||
/// @{
|
||||
|
||||
/// Return a GTSAM rotation
|
||||
const Rotation& rotation() const {
|
||||
return R_;
|
||||
};
|
||||
|
||||
/// Return a GTSAM translation
|
||||
const Translation& translation() const {
|
||||
return t_;
|
||||
};
|
||||
|
||||
/// Return the scale
|
||||
double scale() const {
|
||||
return s_;
|
||||
};
|
||||
|
||||
/// Convert to a rigid body pose
|
||||
operator Pose3() const;
|
||||
|
||||
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() {
|
||||
return 7;
|
||||
};
|
||||
|
||||
/// Dimensionality of tangent space = 7 DOF
|
||||
inline size_t dim() const {
|
||||
return 7;
|
||||
};
|
||||
|
||||
/// @}
|
||||
/// @name Chart
|
||||
/// @{
|
||||
|
||||
/// Update Similarity transform via 7-dim vector in tangent space
|
||||
struct ChartAtOrigin {
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none);
|
||||
|
||||
/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none);
|
||||
};
|
||||
|
||||
/// Project from one tangent space to another
|
||||
Matrix7 AdjointMap() const;
|
||||
|
||||
/// @}
|
||||
/// @name Stubs
|
||||
/// @{
|
||||
|
||||
/// Not currently implemented, required because this is a lie group
|
||||
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
|
||||
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
using LieGroup<Similarity3, 7>::inverse; // version with derivative
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
|
||||
}
|
||||
|
|
@ -0,0 +1,256 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testSimilarity3.cpp
|
||||
* @brief Unit tests for Similarity3 class
|
||||
* @author Paul Drews
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
|
||||
|
||||
static Point3 P(0.2,0.7,-2);
|
||||
static Rot3 R = Rot3::rodriguez(0.3,0,0);
|
||||
static Similarity3 T(R,Point3(3.5,-8.2,4.2),1);
|
||||
static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
|
||||
static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Constructors) {
|
||||
Similarity3 test;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Getters) {
|
||||
Similarity3 test;
|
||||
EXPECT(assert_equal(Rot3(), test.rotation()));
|
||||
EXPECT(assert_equal(Point3(), test.translation()));
|
||||
EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Getters2) {
|
||||
Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
|
||||
EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
|
||||
EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
|
||||
EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
|
||||
}
|
||||
|
||||
TEST(Similarity3, AdjointMap) {
|
||||
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
|
||||
Matrix7 result;
|
||||
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
|
||||
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
|
||||
-2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000,
|
||||
0, 0, 0, -0.2248, -0.3502, -0.9093, 0,
|
||||
0, 0, 0, 0.9024, -0.4269, -0.0587, 0,
|
||||
0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
|
||||
0, 0, 0, 0, 0, 0, 1.0000;
|
||||
EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
|
||||
}
|
||||
|
||||
TEST(Similarity3, inverse) {
|
||||
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
|
||||
Matrix3 Re;
|
||||
Re << -0.2248, 0.9024, -0.3676,
|
||||
-0.3502, -0.4269, -0.8337,
|
||||
-0.9093, -0.0587, 0.4120;
|
||||
Vector3 te(-9.8472, 59.7640, 10.2125);
|
||||
Similarity3 expected(Re, te, 1.0/7.0);
|
||||
EXPECT(assert_equal(expected, test.inverse(), 1e-3));
|
||||
}
|
||||
|
||||
TEST(Similarity3, multiplication) {
|
||||
Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
|
||||
Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
|
||||
Matrix3 re;
|
||||
re << 0.0688, 0.9863, -0.1496,
|
||||
-0.5665, -0.0848, -0.8197,
|
||||
-0.8211, 0.1412, 0.5530;
|
||||
Vector3 te(-13.6797, 3.2441, -5.7794);
|
||||
Similarity3 expected(re, te, 77);
|
||||
EXPECT(assert_equal(expected, test1*test2, 1e-2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Manifold) {
|
||||
EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
|
||||
Vector z = Vector7::Zero();
|
||||
Similarity3 sim;
|
||||
EXPECT(sim.retract(z) == sim);
|
||||
|
||||
Vector7 v = Vector7::Zero();
|
||||
v(6) = 2;
|
||||
Similarity3 sim2;
|
||||
EXPECT(sim2.retract(z) == sim2);
|
||||
|
||||
EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
|
||||
|
||||
Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
|
||||
Vector v3(7);
|
||||
v3 << 0, 0, 0, 1, 2, 3, 0;
|
||||
EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
|
||||
|
||||
// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
|
||||
Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
|
||||
|
||||
Vector vlocal = sim.localCoordinates(other);
|
||||
|
||||
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
|
||||
|
||||
Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
|
||||
Rot3 R = Rot3::rodriguez(0.3,0,0);
|
||||
|
||||
Vector vlocal2 = sim.localCoordinates(other2);
|
||||
|
||||
EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));
|
||||
|
||||
// TODO add unit tests for retract and localCoordinates
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Similarity3, retract_first_order)
|
||||
{
|
||||
Similarity3 id;
|
||||
Vector v = zero(7);
|
||||
v(0) = 0.3;
|
||||
EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2));
|
||||
v(3)=0.2;v(4)=0.7;v(5)=-2;
|
||||
EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Similarity3, localCoordinates_first_order)
|
||||
{
|
||||
Vector d12 = repeat(7,0.1);
|
||||
d12(6) = 1.0;
|
||||
Similarity3 t1 = T, t2 = t1.retract(d12);
|
||||
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Similarity3, manifold_first_order)
|
||||
{
|
||||
Similarity3 t1 = T;
|
||||
Similarity3 t2 = T3;
|
||||
Similarity3 origin;
|
||||
Vector d12 = t1.localCoordinates(t2);
|
||||
EXPECT(assert_equal(t2, t1.retract(d12)));
|
||||
Vector d21 = t2.localCoordinates(t1);
|
||||
EXPECT(assert_equal(t1, t2.retract(d21)));
|
||||
}
|
||||
|
||||
TEST(Similarity3, Optimization) {
|
||||
Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
|
||||
Symbol key('x',1);
|
||||
PriorFactor<Similarity3> factor(key, prior, model);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor);
|
||||
|
||||
Values initial;
|
||||
initial.insert<Similarity3>(key, Similarity3());
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtParams params;
|
||||
params.setVerbosityLM("TRYCONFIG");
|
||||
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
|
||||
}
|
||||
|
||||
TEST(Similarity3, Optimization2) {
|
||||
Similarity3 prior = Similarity3();
|
||||
Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
|
||||
Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
|
||||
Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
|
||||
Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
|
||||
Similarity3 loop = Similarity3(1.42);
|
||||
|
||||
//prior.print("Goal Transform");
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01);
|
||||
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
|
||||
SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
|
||||
PriorFactor<Similarity3> factor(X(1), prior, model);
|
||||
BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
|
||||
BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
|
||||
BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
|
||||
BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
|
||||
BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);
|
||||
|
||||
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor);
|
||||
graph.push_back(b1);
|
||||
graph.push_back(b2);
|
||||
graph.push_back(b3);
|
||||
graph.push_back(b4);
|
||||
graph.push_back(lc);
|
||||
|
||||
//graph.print("Full Graph\n");
|
||||
|
||||
Values initial;
|
||||
initial.insert<Similarity3>(X(1), Similarity3());
|
||||
initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
|
||||
initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
|
||||
initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
|
||||
initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
|
||||
|
||||
//initial.print("Initial Estimate\n");
|
||||
|
||||
Values result;
|
||||
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
//result.print("Optimized Estimate\n");
|
||||
Pose3 p1, p2, p3, p4, p5;
|
||||
p1 = Pose3(result.at<Similarity3>(X(1)));
|
||||
p2 = Pose3(result.at<Similarity3>(X(2)));
|
||||
p3 = Pose3(result.at<Similarity3>(X(3)));
|
||||
p4 = Pose3(result.at<Similarity3>(X(4)));
|
||||
p5 = Pose3(result.at<Similarity3>(X(5)));
|
||||
|
||||
//p1.print("Pose1");
|
||||
//p2.print("Pose2");
|
||||
//p3.print("Pose3");
|
||||
//p4.print("Pose4");
|
||||
//p5.print("Pose5");
|
||||
|
||||
Similarity3 expected(0.7);
|
||||
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
Loading…
Reference in New Issue