Added TransformBtwRobotsUnaryFactorEM. May need to move to MAST later.
Vadim Indelman
parent
f6402916d6
commit
77e02a332e
|
|
@ -0,0 +1,311 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 TransformBtwRobotsUnaryFactorEM.h
|
||||
* @brief Unary factor for determining transformation between given trajectories of two robots
|
||||
* @author Vadim Indelman
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <ostream>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A class for a measurement predicted by "between(config[key1],config[key2])"
|
||||
* @tparam VALUE the Value type
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class VALUE>
|
||||
class TransformBtwRobotsUnaryFactorEM: public NonlinearFactor {
|
||||
|
||||
public:
|
||||
|
||||
typedef VALUE T;
|
||||
|
||||
private:
|
||||
|
||||
typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
|
||||
typedef gtsam::NonlinearFactor Base;
|
||||
|
||||
gtsam::Key key_;
|
||||
|
||||
VALUE measured_; /** The measurement */
|
||||
|
||||
gtsam::Values valA_; // given values for robot A map\trajectory
|
||||
gtsam::Values valB_; // given values for robot B map\trajectory
|
||||
gtsam::Key keyA_; // key of robot A to which the measurement refers
|
||||
gtsam::Key keyB_; // key of robot B to which the measurement refers
|
||||
|
||||
// TODO: create function to update valA_ and valB_
|
||||
|
||||
SharedGaussian model_inlier_;
|
||||
SharedGaussian model_outlier_;
|
||||
|
||||
double prior_inlier_;
|
||||
double prior_outlier_;
|
||||
|
||||
bool flag_bump_up_near_zero_probs_;
|
||||
|
||||
/** concept check by type */
|
||||
GTSAM_CONCEPT_LIE_TYPE(T)
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(T)
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<TransformBtwRobotsUnaryFactorEM> shared_ptr;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
TransformBtwRobotsUnaryFactorEM() {}
|
||||
|
||||
/** Constructor */
|
||||
TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
|
||||
const gtsam::Values valA, const gtsam::Values valB,
|
||||
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
|
||||
const double prior_inlier, const double prior_outlier) :
|
||||
Base(key), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
|
||||
model_inlier_(model_inlier), model_outlier_(model_outlier),
|
||||
prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(false){
|
||||
|
||||
setValAValB(valA, valB);
|
||||
|
||||
}
|
||||
|
||||
virtual ~TransformBtwRobotsUnaryFactorEM() {}
|
||||
|
||||
|
||||
/** Clone */
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
|
||||
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "TransformBtwRobotsUnaryFactorEM("
|
||||
<< keyFormatter(key_) << ")\n";
|
||||
std::cout << "MR between factor keys: "
|
||||
<< keyFormatter(keyA_) << ","
|
||||
<< keyFormatter(keyB_) << "\n";
|
||||
measured_.print(" measured: ");
|
||||
model_inlier_->print(" noise model inlier: ");
|
||||
model_outlier_->print(" noise model outlier: ");
|
||||
std::cout << "(prior_inlier, prior_outlier_) = ("
|
||||
<< prior_inlier_ << ","
|
||||
<< prior_outlier_ << ")\n";
|
||||
// Base::print(s, keyFormatter);
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
|
||||
const This *t = dynamic_cast<const This*> (&f);
|
||||
|
||||
if(t && Base::equals(f))
|
||||
return key_ == t->key_ && measured_.equals(t->measured_) &&
|
||||
// model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
|
||||
// model_outlier_->equals(t->model_outlier_ ) &&
|
||||
prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/* ************************************************************************* */
|
||||
void setValAValB(const gtsam::Values valA, const gtsam::Values valB){
|
||||
if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
|
||||
throw("something is wrong!");
|
||||
|
||||
// TODO: make sure the two keys belong to different robots
|
||||
|
||||
if (valA.exists(keyA_)){
|
||||
valA_ = valA;
|
||||
valB_ = valB;
|
||||
}
|
||||
else {
|
||||
valA_ = valB;
|
||||
valB_ = valA;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
virtual double error(const gtsam::Values& x) const {
|
||||
return whitenedError(x).squaredNorm();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
|
||||
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
|
||||
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
|
||||
*/
|
||||
/* This version of linearize recalculates the noise model each time */
|
||||
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x, const gtsam::Ordering& ordering) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(x))
|
||||
return boost::shared_ptr<gtsam::JacobianFactor>();
|
||||
|
||||
//std::cout<<"About to linearize"<<std::endl;
|
||||
gtsam::Matrix A1;
|
||||
std::vector<gtsam::Matrix> A(this->size());
|
||||
gtsam::Vector b = -whitenedError(x, A);
|
||||
A1 = A[0];
|
||||
|
||||
return gtsam::GaussianFactor::shared_ptr(
|
||||
new gtsam::JacobianFactor(ordering[key_], A1, b, gtsam::noiseModel::Unit::Create(b.size())));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector whitenedError(const gtsam::Values& x,
|
||||
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
|
||||
|
||||
bool debug = true;
|
||||
|
||||
Matrix H_compose, H_between1, H_dummy;
|
||||
|
||||
T orgA_T_currA = valA_.at<T>(keyA_);
|
||||
T orgB_T_currB = valB_.at<T>(keyB_);
|
||||
|
||||
T orgA_T_orgB = x.at<T>(key_);
|
||||
|
||||
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy);
|
||||
|
||||
T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1);
|
||||
|
||||
T currA_T_currB_msr = measured_;
|
||||
|
||||
Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
|
||||
|
||||
// Calculate indicator probabilities (inlier and outlier)
|
||||
Vector p_inlier_outlier = calcIndicatorProb(x);
|
||||
double p_inlier = p_inlier_outlier[0];
|
||||
double p_outlier = p_inlier_outlier[1];
|
||||
|
||||
Vector err_wh_inlier = model_inlier_->whiten(err);
|
||||
Vector err_wh_outlier = model_outlier_->whiten(err);
|
||||
|
||||
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
|
||||
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
|
||||
|
||||
Vector err_wh_eq;
|
||||
err_wh_eq.resize(err_wh_inlier.rows()*2);
|
||||
err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
|
||||
|
||||
Matrix H_unwh = H_compose * H_between1;
|
||||
|
||||
if (H){
|
||||
|
||||
Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
|
||||
Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
|
||||
Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier);
|
||||
|
||||
(*H)[0].resize(H_aug.rows(),H_aug.cols());
|
||||
(*H)[0] = H_aug;
|
||||
}
|
||||
|
||||
if (debug){
|
||||
// std::cout<<"H_compose - rows, cols, : "<<H_compose.rows()<<", "<< H_compose.cols()<<std::endl;
|
||||
// std::cout<<"H_between1 - rows, cols, : "<<H_between1.rows()<<", "<< H_between1.cols()<<std::endl;
|
||||
// std::cout<<"H_unwh - rows, cols, : "<<H_unwh.rows()<<", "<< H_unwh.cols()<<std::endl;
|
||||
// std::cout<<"H_unwh: "<<std:endl<<H_unwh[0]
|
||||
|
||||
}
|
||||
|
||||
|
||||
return err_wh_eq;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
|
||||
|
||||
Vector err = unwhitenedError(x);
|
||||
|
||||
// Calculate indicator probabilities (inlier and outlier)
|
||||
Vector err_wh_inlier = model_inlier_->whiten(err);
|
||||
Vector err_wh_outlier = model_outlier_->whiten(err);
|
||||
|
||||
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
|
||||
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
|
||||
|
||||
double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
|
||||
double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
|
||||
|
||||
double sumP = p_inlier + p_outlier;
|
||||
p_inlier /= sumP;
|
||||
p_outlier /= sumP;
|
||||
|
||||
// if (flag_bump_up_near_zero_probs_){
|
||||
// Bump up near-zero probabilities (as in linerFlow.h)
|
||||
double minP = 0.05; // == 0.1 / 2 indicator variables
|
||||
if (p_inlier < minP || p_outlier < minP){
|
||||
if (p_inlier < minP)
|
||||
p_inlier = minP;
|
||||
if (p_outlier < minP)
|
||||
p_outlier = minP;
|
||||
sumP = p_inlier + p_outlier;
|
||||
p_inlier /= sumP;
|
||||
p_outlier /= sumP;
|
||||
}
|
||||
// }
|
||||
|
||||
return Vector_(2, p_inlier, p_outlier);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
|
||||
|
||||
T orgA_T_currA = valA_.at<T>(keyA_);
|
||||
T orgB_T_currB = valB_.at<T>(keyB_);
|
||||
|
||||
T orgA_T_orgB = x.at<T>(key_);
|
||||
|
||||
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
|
||||
|
||||
T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
|
||||
|
||||
T currA_T_currB_msr = measured_;
|
||||
|
||||
return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
/** number of variables attached to this factor */
|
||||
std::size_t size() const {
|
||||
return 1;
|
||||
}
|
||||
|
||||
virtual size_t dim() const {
|
||||
return model_inlier_->R().rows() + model_inlier_->R().cols();
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
//ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
}
|
||||
}; // \class TransformBtwRobotsUnaryFactorEM
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,335 @@
|
|||
/**
|
||||
* @file testBTransformBtwRobotsUnaryFactorEM.cpp
|
||||
* @brief Unit test for the TransformBtwRobotsUnaryFactorEM
|
||||
* @author Vadim Indelman
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
|
||||
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
|
||||
//#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
|
||||
gtsam::Values values;
|
||||
values.insert(key, org1_T_org2);
|
||||
// LieVector err = factor.whitenedError(values);
|
||||
// return err;
|
||||
return LieVector::Expmap(factor.whitenedError(values));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
|
||||
// gtsam::Values values;
|
||||
// values.insert(keyA, p1);
|
||||
// values.insert(keyB, p2);
|
||||
// // LieVector err = factor.whitenedError(values);
|
||||
// // return err;
|
||||
// return LieVector::Expmap(factor.whitenedError(values));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals)
|
||||
{
|
||||
gtsam::Key key(0);
|
||||
gtsam::Key keyA(1);
|
||||
gtsam::Key keyB(2);
|
||||
|
||||
gtsam::Pose2 p1(10.0, 15.0, 0.1);
|
||||
gtsam::Pose2 p2(15.0, 15.0, 0.3);
|
||||
gtsam::Pose2 noise(0.5, 0.4, 0.01);
|
||||
gtsam::Pose2 rel_pose_ideal = p1.between(p2);
|
||||
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
|
||||
|
||||
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0)));
|
||||
|
||||
double prior_outlier = 0.5;
|
||||
double prior_inlier = 0.5;
|
||||
|
||||
gtsam::Values valA, valB;
|
||||
valA.insert(keyA, p1);
|
||||
valB.insert(keyB, p2);
|
||||
|
||||
// Constructor
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
// Equals
|
||||
CHECK(assert_equal(g, h, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
|
||||
{
|
||||
gtsam::Key key(0);
|
||||
gtsam::Key keyA(1);
|
||||
gtsam::Key keyB(2);
|
||||
|
||||
gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
|
||||
gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
|
||||
|
||||
gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
|
||||
|
||||
gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
|
||||
|
||||
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
|
||||
gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
|
||||
|
||||
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0)));
|
||||
|
||||
double prior_outlier = 0.01;
|
||||
double prior_inlier = 0.99;
|
||||
|
||||
gtsam::Values valA, valB;
|
||||
valA.insert(keyA, orgA_T_1);
|
||||
valB.insert(keyB, orgB_T_2);
|
||||
|
||||
// Constructor
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
gtsam::Values values;
|
||||
values.insert(key, orgA_T_orgB);
|
||||
Vector err = g.unwhitenedError(values);
|
||||
|
||||
// Equals
|
||||
CHECK(assert_equal(err, zero(3), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
|
||||
{
|
||||
gtsam::Key key(0);
|
||||
gtsam::Key keyA(1);
|
||||
gtsam::Key keyB(2);
|
||||
|
||||
gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
|
||||
gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
|
||||
|
||||
gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
|
||||
|
||||
gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
|
||||
|
||||
gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB);
|
||||
gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
|
||||
|
||||
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0)));
|
||||
|
||||
double prior_outlier = 0.01;
|
||||
double prior_inlier = 0.99;
|
||||
|
||||
gtsam::Values valA, valB;
|
||||
valA.insert(keyA, orgA_T_currA);
|
||||
valB.insert(keyB, orgB_T_currB);
|
||||
|
||||
// Constructor
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
gtsam::Values values;
|
||||
values.insert(key, orgA_T_orgB);
|
||||
Vector err = g.unwhitenedError(values);
|
||||
|
||||
// Equals
|
||||
CHECK(assert_equal(err, zero(3), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformBtwRobotsUnaryFactorEM, Optimize)
|
||||
{
|
||||
gtsam::Key key(0);
|
||||
gtsam::Key keyA(1);
|
||||
gtsam::Key keyB(2);
|
||||
|
||||
gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
|
||||
gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
|
||||
|
||||
gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
|
||||
gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB);
|
||||
gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr);
|
||||
|
||||
// some error in measurements
|
||||
// gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
|
||||
// gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
|
||||
// gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
|
||||
// gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
|
||||
|
||||
// ideal measurements
|
||||
gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0));
|
||||
gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
|
||||
gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
|
||||
gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
|
||||
|
||||
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0)));
|
||||
|
||||
double prior_outlier = 0.01;
|
||||
double prior_inlier = 0.99;
|
||||
|
||||
gtsam::Values valA, valB;
|
||||
valA.insert(keyA, orgA_T_currA);
|
||||
valB.insert(keyB, orgB_T_currB);
|
||||
|
||||
// Constructor
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
gtsam::Values values;
|
||||
values.insert(key, gtsam::Pose2());
|
||||
|
||||
gtsam::NonlinearFactorGraph graph;
|
||||
graph.add(g1);
|
||||
graph.add(g2);
|
||||
graph.add(g3);
|
||||
graph.add(g4);
|
||||
|
||||
gtsam::GaussNewtonParams params;
|
||||
gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
|
||||
gtsam::Values result = optimizer.optimize();
|
||||
|
||||
gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(key);
|
||||
|
||||
CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
|
||||
{
|
||||
gtsam::Key key(0);
|
||||
gtsam::Key keyA(1);
|
||||
gtsam::Key keyB(2);
|
||||
|
||||
gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
|
||||
gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
|
||||
|
||||
gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
|
||||
|
||||
gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
|
||||
|
||||
gtsam::Pose2 noise(0.5, 0.4, 0.01);
|
||||
|
||||
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
|
||||
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
|
||||
|
||||
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0)));
|
||||
|
||||
double prior_outlier = 0.5;
|
||||
double prior_inlier = 0.5;
|
||||
|
||||
gtsam::Values valA, valB;
|
||||
valA.insert(keyA, orgA_T_1);
|
||||
valB.insert(keyB, orgB_T_2);
|
||||
|
||||
// Constructor
|
||||
TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
|
||||
model_inlier, model_outlier,prior_inlier, prior_outlier);
|
||||
|
||||
gtsam::Values values;
|
||||
values.insert(key, orgA_T_orgB);
|
||||
|
||||
std::vector<gtsam::Matrix> H_actual(1);
|
||||
Vector actual_err_wh = g.whitenedError(values, H_actual);
|
||||
|
||||
Matrix H1_actual = H_actual[0];
|
||||
|
||||
double stepsize = 1.0e-9;
|
||||
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
|
||||
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
|
||||
}
|
||||
/////* ************************************************************************** */
|
||||
//TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) {
|
||||
//
|
||||
// gtsam::Key keyA(1);
|
||||
// gtsam::Key keyB(2);
|
||||
//
|
||||
// // Inlier test
|
||||
// gtsam::Pose2 p1(10.0, 15.0, 0.1);
|
||||
// gtsam::Pose2 p2(15.0, 15.0, 0.3);
|
||||
// gtsam::Pose2 noise(0.5, 0.4, 0.01);
|
||||
// gtsam::Pose2 rel_pose_ideal = p1.between(p2);
|
||||
// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
|
||||
//
|
||||
// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05)));
|
||||
// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0)));
|
||||
//
|
||||
// gtsam::Values values;
|
||||
// values.insert(keyA, p1);
|
||||
// values.insert(keyB, p2);
|
||||
//
|
||||
// double prior_outlier = 0.0;
|
||||
// double prior_inlier = 1.0;
|
||||
//
|
||||
// TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
|
||||
// prior_inlier, prior_outlier);
|
||||
//
|
||||
// std::vector<gtsam::Matrix> H_actual(2);
|
||||
// Vector actual_err_wh = f.whitenedError(values, H_actual);
|
||||
//
|
||||
// Matrix H1_actual = H_actual[0];
|
||||
// Matrix H2_actual = H_actual[1];
|
||||
//
|
||||
// // compare to standard between factor
|
||||
// BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
|
||||
// Vector actual_err_wh_stnd = h.whitenedError(values);
|
||||
// Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
|
||||
// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
|
||||
// std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
|
||||
// (void)h.unwhitenedError(values, H_actual_stnd_unwh);
|
||||
// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
|
||||
// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
|
||||
// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
|
||||
// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
|
||||
//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
|
||||
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
|
||||
//
|
||||
// double stepsize = 1.0e-9;
|
||||
// Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
|
||||
// Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
|
||||
//
|
||||
//
|
||||
// // try to check numerical derivatives of a standard between factor
|
||||
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
|
||||
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
|
||||
//
|
||||
//
|
||||
// CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
|
||||
// CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue