131 lines
4.3 KiB
C++
131 lines
4.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 testRot3.cpp
|
|
* @brief Unit tests for Rot3Q class
|
|
* @author Richard Roberts
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <gtsam/base/Testable.h>
|
|
#include <boost/math/constants/constants.hpp>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/base/lieProxies.h>
|
|
#include <gtsam/geometry/Point3.h>
|
|
#include <gtsam/geometry/Rot3Q.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
|
#include <gtsam/nonlinear/Values-inl.h>
|
|
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
|
|
using namespace gtsam;
|
|
|
|
typedef TypedSymbol<Rot3Q, 'r'> KeyQ;
|
|
typedef Values<KeyQ> ValuesQ;
|
|
typedef PriorFactor<ValuesQ, KeyQ> PriorQ;
|
|
typedef BetweenFactor<ValuesQ, KeyQ> BetweenQ;
|
|
typedef NonlinearFactorGraph<ValuesQ> GraphQ;
|
|
|
|
typedef TypedSymbol<Rot3M, 'r'> KeyM;
|
|
typedef Values<KeyM> ValuesM;
|
|
typedef PriorFactor<ValuesM, KeyM> PriorM;
|
|
typedef BetweenFactor<ValuesM, KeyM> BetweenM;
|
|
typedef NonlinearFactorGraph<ValuesM> GraphM;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(Rot3Q, optimize1) {
|
|
GraphQ fgQ;
|
|
fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
|
|
fgQ.add(BetweenQ(0, 1, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
|
fgQ.add(BetweenQ(1, 0, Rot3Q::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
|
|
|
|
GraphM fgM;
|
|
fgM.add(PriorM(0, Rot3M(), sharedSigma(3, 0.01)));
|
|
fgM.add(BetweenM(0, 1, Rot3M::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
|
fgM.add(BetweenM(1, 0, Rot3M::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
|
|
|
|
ValuesQ initialQ;
|
|
initialQ.insert(0, Rot3Q::Rz(0.0));
|
|
initialQ.insert(1, Rot3Q::Rz(M_PI/3.0 + 0.1));
|
|
|
|
ValuesM initialM;
|
|
initialM.insert(0, Rot3M::Rz(0.0));
|
|
initialM.insert(1, Rot3M::Rz(M_PI/3.0 + 0.1));
|
|
|
|
ValuesQ truthQ;
|
|
truthQ.insert(0, Rot3Q::Rz(0.0));
|
|
truthQ.insert(1, Rot3Q::Rz(M_PI/3.0));
|
|
|
|
ValuesM truthM;
|
|
truthM.insert(0, Rot3M::Rz(0.0));
|
|
truthM.insert(1, Rot3M::Rz(M_PI/3.0));
|
|
|
|
// Compare Matrix and Quaternion between
|
|
Matrix H1M, H2M;
|
|
Rot3M betwM = initialM[1].between(initialM[0], H1M, H2M);
|
|
Matrix H1Q, H2Q;
|
|
Rot3Q betwQ = initialM[1].between(initialM[0], H1Q, H2Q);
|
|
EXPECT(assert_equal(betwM.matrix(), betwQ.matrix()));
|
|
EXPECT(assert_equal(H1M, H1Q));
|
|
EXPECT(assert_equal(H2M, H2Q));
|
|
Point3 x1(1.0,0.0,0.0), x2(0.0,1.0,0.0);
|
|
EXPECT(assert_equal(betwM*x1, betwQ*x1));
|
|
EXPECT(assert_equal(betwM*x2, betwQ*x2));
|
|
|
|
// Compare Matrix and Quaternion logmap
|
|
Vector logM = initialM[1].localCoordinates(initialM[0]);
|
|
Vector logQ = initialQ[1].localCoordinates(initialQ[0]);
|
|
EXPECT(assert_equal(logM, logQ));
|
|
|
|
// Compare Matrix and Quaternion linear graph
|
|
Ordering ordering; ordering += KeyQ(0), KeyQ(1);
|
|
GaussianFactorGraph gfgQ(*fgQ.linearize(initialQ, ordering));
|
|
GaussianFactorGraph gfgM(*fgM.linearize(initialM, ordering));
|
|
EXPECT(assert_equal(gfgQ, gfgM, 1e-5));
|
|
|
|
NonlinearOptimizationParameters params;
|
|
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
|
|
ValuesQ final = optimize(fgQ, initialQ, params);
|
|
|
|
EXPECT(assert_equal(truthQ, final, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(Rot3Q, optimize) {
|
|
|
|
// Optimize a circle
|
|
ValuesQ truth;
|
|
ValuesQ initial;
|
|
GraphQ fg;
|
|
fg.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
|
|
for(int j=0; j<6; ++j) {
|
|
truth.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j)));
|
|
initial.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
|
|
fg.add(BetweenQ(j, (j+1)%6, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
|
}
|
|
|
|
NonlinearOptimizationParameters params;
|
|
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
|
|
ValuesQ final = optimize(fg, initial, params);
|
|
|
|
EXPECT(assert_equal(truth, final, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|