void populateTestPoseGraph(PoseGraph & graph)
{
// Build a sample pose graph.
// 2 | --> 12 --> 13 --> 14 <-- 15 --> 16
// | / A
// 1 | /-> 6 --> 7 --> 8 \-----
// | / V |
// Y 0 | 1 --> 2 <-- 3 -> 9 --> 10 --> 11
// | \ /
// -1 | \-> 4 --> 5 / 17 --> 18 --> 19
// |
// ------------------------------------------------------------
// 0 1 2 3 4 5 6 7 8
// X
// Add the vertices.
for(int i = 1; i <= 19; i++)
{
graph.addVertex(VertexId(i));
}
Eigen::Matrix<double,6,1> udiag;
udiag << 0.1 ,0.1 ,0.1, // Translation
0.01,0.01,0.01; // Rotation
Eigen::Matrix<double,6,6> U = udiag.asDiagonal();
// Add the edges: addEdge(to,from,T_to_from
graph.addEdge (VertexId(2), VertexId(1), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*1));
graph.addEdge (VertexId(2), VertexId(3), UncertainTransformation(toTEuler( 1, 0, 0, 0, 0, 0), U*2));
graph.addEdge (VertexId(4), VertexId(3), UncertainTransformation(toTEuler(-1, 1, 0, 0, 0, 0), U*3));
graph.addEdge (VertexId(6), VertexId(3), UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*4));
graph.addEdge (VertexId(5), VertexId(4), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*5));
graph.addEdge (VertexId(7), VertexId(6), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*6));
graph.addEdge (VertexId(8), VertexId(7), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*7));
graph.addEdge (VertexId(9), VertexId(8), UncertainTransformation(toTEuler( 0, 1, 0, 0, 0, 0), U*8));
graph.addEdge (VertexId(9), VertexId(5), UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*9));
graph.addEdge (VertexId(10), VertexId(9), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*10));
graph.addEdge (VertexId(11), VertexId(10), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*11));
graph.addEdge (VertexId(14), VertexId(11), UncertainTransformation(toTEuler( 1,-2, 0, 0, 0, 0), U*12));
graph.addEdge (VertexId(12), VertexId(6), UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*13));
graph.addEdge (VertexId(13), VertexId(12), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*14));
graph.addEdge (VertexId(14), VertexId(13), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*15));
graph.addEdge (VertexId(14), VertexId(15), UncertainTransformation(toTEuler( 1, 0, 0, 0, 0, 0), U*16));
graph.addEdge (VertexId(16), VertexId(15), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*17));
graph.addEdge (VertexId(18), VertexId(17), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*18));
graph.addEdge (VertexId(19), VertexId(18), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*19));
// This works.
}
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
multi_gp_log(const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
const Eigen::Matrix
<T_covar, Eigen::Dynamic, Eigen::Dynamic>& Sigma,
const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
static const char* function("multi_gp_log");
typedef typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
T_lp;
T_lp lp(0.0);
check_positive(function, "Kernel rows", Sigma.rows());
check_finite(function, "Kernel", Sigma);
check_symmetric(function, "Kernel", Sigma);
LDLT_factor<T_covar, Eigen::Dynamic, Eigen::Dynamic> ldlt_Sigma(Sigma);
check_ldlt_factor(function, "LDLT_Factor of Sigma", ldlt_Sigma);
check_size_match(function,
"Size of random variable (rows y)", y.rows(),
"Size of kernel scales (w)", w.size());
check_size_match(function,
"Size of random variable", y.cols(),
"rows of covariance parameter", Sigma.rows());
check_positive_finite(function, "Kernel scales", w);
check_finite(function, "Random variable", y);
if (y.rows() == 0)
return lp;
if (include_summand<propto>::value) {
lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
}
if (include_summand<propto, T_covar>::value) {
lp -= 0.5 * log_determinant_ldlt(ldlt_Sigma) * y.rows();
}
if (include_summand<propto, T_w>::value) {
lp += (0.5 * y.cols()) * sum(log(w));
}
if (include_summand<propto, T_y, T_w, T_covar>::value) {
Eigen::Matrix<T_w, Eigen::Dynamic, Eigen::Dynamic>
w_mat(w.asDiagonal());
Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic> yT(y.transpose());
lp -= 0.5 * trace_gen_inv_quad_form_ldlt(w_mat, ldlt_Sigma, yT);
}
return lp;
}
ErrorTermTransformation::ErrorTermTransformation(aslam::backend::TransformationExpression T, sm::kinematics::Transformation prior, double weightRotation, double weightTranslation) :
_T(T), _prior(prior), _debug(0)
{
Eigen::Matrix<double, 6, 1> W;
W << weightTranslation, weightTranslation, weightTranslation, weightRotation, weightRotation, weightRotation;
// Fill in the inverse covariance.
setInvR(Eigen::Matrix<double,6,6>(W.asDiagonal()));
// Tell the super class about the design variables:
aslam::backend::DesignVariable::set_t dv;
_T.getDesignVariables(dv);
setDesignVariablesIterator(dv.begin(), dv.end());
}
void createRotationProblems
(
size_t nr_problems,
float max_angle,
float max_compression,
Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
Vcl::Core::InterleavedArray<float, 3, 3, -1>* R
)
{
// Random number generator
std::mt19937_64 rng;
std::uniform_real_distribution<float> d;
std::uniform_real_distribution<float> a{ -max_angle, max_angle };
for (int i = 0; i < (int)nr_problems; i++)
{
// Rest-state
Eigen::Matrix3f X0;
X0 << d(rng), d(rng), d(rng),
d(rng), d(rng), d(rng),
d(rng), d(rng), d(rng);
// Rotation angle
float angle = a(rng);
// Rotation axis
Eigen::Matrix<float, 3, 1> rot_vec;
rot_vec << d(rng), d(rng), d(rng);
rot_vec.normalize();
// Rotation matrix
Eigen::Matrix3f Rot = Eigen::AngleAxis<float>{ angle, rot_vec }.toRotationMatrix();
if (R)
R->template at<float>(i) = Rot;
if (max_compression > 0)
{
Eigen::Matrix<float, 3, 1> scaling;
scaling << (1.0f - max_compression*d(rng)), (1.0f - max_compression*d(rng)), (1.0f - max_compression*d(rng));
Eigen::JacobiSVD<Eigen::Matrix3f> svd{ Rot, Eigen::ComputeFullU | Eigen::ComputeFullV };
Rot *= svd.matrixV() * scaling.asDiagonal() * svd.matrixV().transpose();
}
Eigen::Matrix3f X = Rot * X0;
F.at<float>(i) = X * X0.inverse();
}
}
typename boost::math::tools::promote_args<T_y,T_w,T_covar>::type
multi_gp_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& Sigma,
const Eigen::Matrix<T_w,Eigen::Dynamic,1>& w) {
static const char* function = "stan::prob::multi_gp_log(%1%)";
typedef typename boost::math::tools::promote_args<T_y,T_w,T_covar>::type T_lp;
T_lp lp(0.0);
using stan::math::log;
using stan::math::sum;
using stan::math::check_not_nan;
using stan::math::check_size_match;
using stan::math::check_positive;
using stan::math::check_finite;
using stan::math::check_symmetric;
using stan::math::dot_product;
using stan::math::rows_dot_product;
using stan::math::log_determinant_ldlt;
using stan::math::mdivide_right_ldlt;
using stan::math::trace_gen_inv_quad_form_ldlt;
using stan::math::LDLT_factor;
using stan::math::check_ldlt_factor;
if (!check_size_match(function,
Sigma.rows(), "Rows of kernel matrix",
Sigma.cols(), "columns of kernel matrix",
&lp))
return lp;
if (!check_positive(function, Sigma.rows(), "Kernel rows", &lp))
return lp;
if (!check_finite(function, Sigma, "Kernel", &lp))
return lp;
if (!check_symmetric(function, Sigma, "Kernel", &lp))
return lp;
LDLT_factor<T_covar,Eigen::Dynamic,Eigen::Dynamic> ldlt_Sigma(Sigma);
if(!check_ldlt_factor(function,ldlt_Sigma,"LDLT_Factor of Sigma",&lp))
return lp;
if (!check_size_match(function,
y.rows(), "Size of random variable (rows y)",
w.size(), "Size of kernel scales (w)",
&lp))
return lp;
if (!check_size_match(function,
y.cols(), "Size of random variable",
Sigma.rows(), "rows of covariance parameter",
&lp))
return lp;
if (!check_finite(function, w, "Kernel scales", &lp))
return lp;
if (!check_positive(function, w, "Kernel scales", &lp))
return lp;
if (!check_finite(function, y, "Random variable", &lp))
return lp;
if (y.rows() == 0)
return lp;
if (include_summand<propto>::value) {
lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
}
if (include_summand<propto,T_covar>::value) {
lp -= (0.5 * y.rows()) * log_determinant_ldlt(ldlt_Sigma);
}
if (include_summand<propto,T_w>::value) {
lp += (0.5 * y.cols()) * sum(log(w));
}
if (include_summand<propto,T_y,T_w,T_covar>::value) {
Eigen::Matrix<T_w,Eigen::Dynamic,Eigen::Dynamic> w_mat(w.asDiagonal());
Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic> yT(y.transpose());
lp -= 0.5 * trace_gen_inv_quad_form_ldlt(w_mat,ldlt_Sigma,yT);
}
return lp;
}
void
EstimatorFull::SetCovarianceDiagonal(const Eigen::Matrix<double,28,1> &P_)
{
P = P_.asDiagonal();
}
