Go to the documentation of this file.
75 typedef typename std::shared_ptr<TransformBtwRobotsUnaryFactorEM>
shared_ptr;
84 const double prior_inlier,
const double prior_outlier,
85 const bool flag_bump_up_near_zero_probs =
false,
86 const bool start_with_M_step =
false) :
107 std::cout <<
s <<
"TransformBtwRobotsUnaryFactorEM("
108 << keyFormatter(
key_) <<
")\n";
109 std::cout <<
"MR between factor keys: "
110 << keyFormatter(
keyA_) <<
","
111 << keyFormatter(
keyB_) <<
"\n";
115 std::cout <<
"(prior_inlier, prior_outlier_) = ("
123 const This *
t =
dynamic_cast<const This*
> (&
f);
139 throw(
"something is wrong!");
168 return std::shared_ptr<JacobianFactor>();
172 std::vector<Matrix>
A(this->
size());
189 Matrix H_compose, H_between1, H_dummy;
196 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy);
198 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1);
202 Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
206 double p_inlier = p_inlier_outlier[0];
207 double p_outlier = p_inlier_outlier[1];
223 err_wh_eq.resize(err_wh_inlier.rows()*2);
224 err_wh_eq <<
sqrt(p_inlier) * err_wh_inlier.array() ,
sqrt(p_outlier) * err_wh_outlier.array();
226 Matrix H_unwh = H_compose * H_between1;
234 (*H)[0].resize(H_aug.rows(),H_aug.cols());
273 double p_inlier =
prior_inlier_ *
sqrt(invCov_inlier.norm()) *
exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
274 double p_outlier =
prior_outlier_ *
sqrt(invCov_outlier.norm()) *
exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
276 double sumP = p_inlier + p_outlier;
283 if (p_inlier < minP || p_outlier < minP){
286 if (p_outlier < minP)
288 sumP = p_inlier + p_outlier;
294 return (
Vector(2) << p_inlier, p_outlier).finished();
305 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
307 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
311 return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
339 Keys.push_back(
keyA_);
340 Keys.push_back(
keyB_);
382 p1.between(
p2, H1, H2);
385 H.resize(H1.rows(), H1.rows()+H2.rows());
389 joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
390 joint_cov << cov1, cov12,
391 cov12.transpose(), cov2;
393 Matrix cov_state =
H*joint_cov*
H.transpose();
411 size_t dim()
const override {
417 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
419 friend class boost::serialization::access;
420 template<
class ARCHIVE>
421 void serialize(ARCHIVE & ar,
const unsigned int ) {
422 ar & boost::serialization::make_nvp(
"NonlinearFactor",
423 boost::serialization::base_object<Base>(*
this));
430 template<
class VALUE>
432 public Testable<TransformBtwRobotsUnaryFactorEM<VALUE> > {
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
std::shared_ptr< This > shared_ptr
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Concept check for values that can be used in unit tests.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
virtual bool active(const Values &) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
#define GTSAM_CONCEPT_LIE_TYPE(T)
const ValueType at(Key j) const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Matrix stack(size_t nrMatrices,...)
static constexpr bool debug
std::shared_ptr< This > shared_ptr
shared_ptr to this class
static shared_ptr Create(size_t dim)
noiseModel::Gaussian::shared_ptr SharedGaussian
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
std::vector< Matrix > * OptionalMatrixVecType
Base class and basic functions for Lie types.
A class for computing marginals in a NonlinearFactorGraph.
Factor Graph consisting of non-linear factors.
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
A factor with a quadratic error function - a Gaussian.
NonlinearFactorGraph graph
Marginals marginals(graph, result)
std::uint64_t Key
Integer nonlinear key type.
Jet< T, N > sqrt(const Jet< T, N > &f)
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:32