Permalink
Browse files

fix monumental error in Hessian calculation

cc #770
  • Loading branch information...
jlblancoc committed May 24, 2018
1 parent 8786bd6 commit d63818bc8e4cbe21726649e1a7d94309a6bc1b27
@@ -52,7 +52,7 @@ namespace mrpt::graphslam
*iterations.
* - "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial
*lambda value for the lev-marq algorithm.
* - "tau": (default=1e-3) Initial tau value for the lev-marq algorithm.
* - "tau": (default=1e-6) Initial tau value for the lev-marq algorithm.
* - "e1": (default=1e-6) Lev-marq algorithm iteration stopping criterion
*#1:
*|gradient| < e1
@@ -245,7 +245,6 @@ void optimize_graph_spa_levmarq(
grad.setZero();
using map_ID2matrix_VxV_t =
mrpt::aligned_std_map<TNodeID, typename gst::matrix_VxV_t>;
vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
double lambda = initial_lambda; // Will be actually set on first iteration.
double v = 1; // was 2, changed since it's modified in the first pass.
@@ -260,6 +259,7 @@ void optimize_graph_spa_levmarq(
for (size_t iter = 0; iter < max_iters; ++iter)
{
vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
last_iter = iter;
// This will be false only when the delta leads to a worst solution and
@@ -411,7 +411,7 @@ void optimize_graph_spa_levmarq(
J1, J2, JtJ, lstObservationData[idxObs].edge);
// We sort IDs such as "i" < "j" and we can build just
// the upper triangular part of the Hessian:
if (Hij_upper_triang) // H_map[col][row]
if (Hij_upper_triang) // H_map[col][row]
H_map[idx_j][idx_i] += JtJ;
else
H_map[idx_i][idx_j] += JtJ.transpose();
@@ -499,9 +499,11 @@ void optimize_graph_spa_levmarq(
it->second.get_unsafe(r, r) + lambda);
// c>r:
for (size_t c = r + 1; c < DIMS_POSE; c++)
{
sp_H.insert_entry_fast(
j_offset + r, i_offset + c,
it->second.get_unsafe(r, c));
}
}
}
else
@@ -602,24 +604,19 @@ void optimize_graph_spa_levmarq(
nodes_to_optimize->begin();
it != nodes_to_optimize->end(); ++it)
{
// exp_delta_i = Exp_SE( delta_i )
typename gst::graph_t::constraint_t::type_value
exp_delta_pose(UNINITIALIZED_POSE);
typename gst::Array_O exp_delta;
for (size_t i = 0; i < DIMS_POSE; i++)
exp_delta[i] = -*delta_ptr++; // The "-" sign is for
// the missing "-"
// carried all this time
// from above
gst::SE_TYPE::exp(exp_delta, exp_delta_pose);
exp_delta[i] = -*delta_ptr++;
// The "-" above is for the missing "-" dragged from the
// Gauss-Newton formula above.
// new_x_i = exp_delta_i (+) old_x_i
typename gst::graph_t::global_poses_t::iterator
it_old_value = graph.nodes.find(*it);
old_poses_backup[*it] =
it_old_value->second; // back up the old pose as a copy
it_old_value->second.composeFrom(
exp_delta_pose, it_old_value->second);
detail::AuxPoseOPlus<typename gst::edge_t, gst>::sumIncr(
it_old_value->second, exp_delta);
}
}
@@ -22,6 +22,67 @@ using namespace mrpt::graphslam;
using namespace mrpt::math;
using namespace std;
// Auxiliary struct to update the oplus increments after each iteration
// Specializations are below.
template <class POSE, class gst>
struct AuxPoseOPlus;
// Nodes: CPose2D
template <class gst>
struct AuxPoseOPlus<CPose2D, gst>
{
static inline void sumIncr(CPose2D& p, const typename gst::Array_O& delta)
{
p.x_incr(delta[0]);
p.y_incr(delta[1]);
p.phi_incr(delta[2]);
p.normalizePhi();
}
};
// Nodes: CPosePDFGaussianInf
template <class gst>
struct AuxPoseOPlus<CPosePDFGaussianInf, gst>
{
template <class POSE>
static inline void sumIncr(POSE& p, const typename gst::Array_O& delta)
{
p.x_incr(delta[0]);
p.y_incr(delta[1]);
p.phi_incr(delta[2]);
p.normalizePhi();
}
};
// Nodes: CPose2D
template <class gst>
struct AuxPoseOPlus<CPose3D, gst>
{
static inline void sumIncr(CPose3D& p, const typename gst::Array_O& delta)
{
// exp_delta_i = Exp_SE( delta_i )
typename gst::graph_t::constraint_t::type_value exp_delta_pose(
UNINITIALIZED_POSE);
gst::SE_TYPE::exp(delta, exp_delta_pose);
p = p + exp_delta_pose;
}
};
// Nodes: CPosePDFGaussianInf
template <class gst>
struct AuxPoseOPlus<CPose3DPDFGaussianInf, gst>
{
template <class POSE>
static inline void sumIncr(POSE& p, const typename gst::Array_O& delta)
{
// exp_delta_i = Exp_SE( delta_i )
typename gst::graph_t::constraint_t::type_value exp_delta_pose(
UNINITIALIZED_POSE);
gst::SE_TYPE::exp(delta, exp_delta_pose);
p = p + exp_delta_pose;
}
};
// An auxiliary struct to compute the pseudo-ln of a pose error, possibly
// modified with an information matrix.
// Specializations are below.
@@ -34,18 +95,18 @@ struct AuxErrorEval<CPose2D, gst>
{
template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError(
const POSE& P1DP2inv, VEC& err, const EDGE_ITERATOR& edge)
const POSE& DinvP1invP2, VEC& err, const EDGE_ITERATOR& edge)
{
MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
}
template <class MAT, class EDGE_ITERATOR>
static inline void multiplyJtLambdaJ(
const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
{
MRPT_UNUSED_PARAM(edge);
JtJ = J1.transpose()*J1;
JtJ = J1.transpose() * J1;
}
template <class MAT, class EDGE_ITERATOR>
@@ -61,7 +122,8 @@ struct AuxErrorEval<CPose2D, gst>
const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
{
MRPT_UNUSED_PARAM(edge);
OUT += J.transpose() * ERR;
const auto grad_incr = (J.transpose() * ERR).eval();
OUT += grad_incr;
}
};
@@ -71,10 +133,10 @@ struct AuxErrorEval<CPose3D, gst>
{
template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError(
const POSE& P1DP2inv, VEC& err, const EDGE_ITERATOR& edge)
const POSE& DinvP1invP2, VEC& err, const EDGE_ITERATOR& edge)
{
MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
}
template <class MAT, class EDGE_ITERATOR>
@@ -108,9 +170,9 @@ struct AuxErrorEval<CPosePDFGaussianInf, gst>
{
template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError(
const POSE& P1DP2inv, VEC& err, const EDGE_ITERATOR& edge)
const POSE& DinvP1invP2, VEC& err, const EDGE_ITERATOR& edge)
{
gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
}
template <class MAT, class EDGE_ITERATOR>
@@ -140,10 +202,10 @@ struct AuxErrorEval<CPose3DPDFGaussianInf, gst>
{
template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError(
const POSE& P1DP2inv, VEC& err, const EDGE_ITERATOR& edge)
const POSE& DinvP1invP2, VEC& err, const EDGE_ITERATOR& edge)
{
MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
}
template <class MAT, class EDGE_ITERATOR>
@@ -201,31 +263,24 @@ double computeJacobiansAndErrors(
const auto& edge = e->second;
// Compute the residual pose error of these pair of nodes + its
// constraint,
// that is: P1DP2inv = P1 * EDGE * inv(P2)
typename gst::graph_t::constraint_t::type_value P1DP2inv(
mrpt::poses::UNINITIALIZED_POSE);
{
typename gst::graph_t::constraint_t::type_value P1D(
mrpt::poses::UNINITIALIZED_POSE);
P1D.composeFrom(*P1, *EDGE_POSE);
const typename gst::graph_t::constraint_t::type_value P2inv =
-(*P2); // Pose inverse (NOT just switching signs!)
P1DP2inv.composeFrom(P1D, P2inv);
}
// constraint:
// DinvP1invP2 = inv(EDGE) * inv(P1) * P2 = (P2 \ominus P1) \ominus EDGE
typename gst::graph_t::constraint_t::type_value DinvP1invP2 =
((*P2) - (*P1)) - *EDGE_POSE;
// Add to vector of errors:
errs.resize(errs.size() + 1);
detail::AuxErrorEval<typename gst::edge_t, gst>::computePseudoLnError(
P1DP2inv, errs.back(), edge);
DinvP1invP2, errs.back(), edge);
// Compute the jacobians:
alignas(MRPT_MAX_ALIGN_BYTES)
std::pair<mrpt::graphs::TPairNodeIDs, typename gst::TPairJacobs>
newMapEntry;
newMapEntry.first = ids;
gst::SE_TYPE::jacobian_dP1DP2inv_depsilon(
P1DP2inv, &newMapEntry.second.first, &newMapEntry.second.second);
gst::SE_TYPE::jacobian_dDinvP1invP2_depsilon(
-(*EDGE_POSE), *P1, *P2, &newMapEntry.second.first,
&newMapEntry.second.second);
// And insert into map of jacobians:
lstJacobians.insert(lstJacobians.end(), newMapEntry);
@@ -27,12 +27,15 @@ using namespace mrpt::math;
using namespace std;
// Define in/out files for testing:
using in_out_filenames = std::tuple<std::string, std::string>;
using in_out_filenames = std::set<std::tuple<std::string, std::string>>;
const std::map<std::string, in_out_filenames> inout_graph_files{
{"GraphTester2D",
{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"}},
{{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"}}},
{"GraphTester2DInf",
{"graphslam_SE2pdf_in.graph", "graphslam_SE2pdf_out_good.graph"}}};
{
{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"},
//{"graphslam_SE2pdf_in.graph", "graphslam_SE2pdf_out_good.graph"}
}}};
template <class my_graph_t>
class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
@@ -53,7 +56,6 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
// Run graph slam:
// ----------------------------
mrpt::system::TParametersDouble params;
// params["verbose"] = 1;
params["max_iterations"] = 100;
graphslam::TResultInfoSpaLevMarq levmarq_info;
@@ -65,7 +67,7 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
const double err_end = graph.chi2();
// Do some basic checks on the results:
EXPECT_GE(levmarq_info.num_iters, 10U);
EXPECT_GE(levmarq_info.num_iters, 2U);
EXPECT_LE(levmarq_info.final_total_sq_error, 5e-2);
EXPECT_LT(err_end, err_init);
@@ -115,9 +117,10 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
.abs()
.maxCoeff(),
eps_node_pos)
<< "Poses of keyframe #" << itn1->first << " do not match:" << std::endl
<< "- Expected: " << itn2->second
<< std::endl << "- Got : " << itn1->second << std::endl;
<< "Poses of keyframe #" << itn1->first
<< " do not match:" << std::endl
<< "- Expected: " << itn2->second << std::endl
<< "- Got : " << itn1->second << std::endl;
}
}
}
@@ -160,46 +163,51 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
return; // No tests for this type
const string prefix = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/");
const string in_f = prefix + std::get<0>(files_it->second);
ASSERT_FILE_EXISTS_(in_f);
const string good_f = prefix + std::get<1>(files_it->second);
ASSERT_FILE_EXISTS_(good_f);
my_graph_t graph, graph_good;
graph.loadFromTextFile(in_f);
graph_good.loadFromTextFile(good_f);
ASSERT_(graph.nodeCount() > 1);
ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
// Optimize:
const my_graph_t graph_initial = graph;
mrpt::system::TParametersDouble params;
// params["verbose"] = 1;
params["max_iterations"] = 100;
graphslam::TResultInfoSpaLevMarq levmarq_info;
graphslam::optimize_graph_spa_levmarq(
graph, levmarq_info, nullptr, params);
const double err_init = graph_initial.chi2();
const double err_end = graph.chi2();
const double err_good = graph_good.chi2();
/* DEBUG */
for (const auto& tst : files_it->second)
{
std::cout << "Testing graph type `" << type << "`, in_file=`"
<< std::get<0>(tst) << "`" << std::endl;
const string in_f = prefix + std::get<0>(tst);
ASSERT_FILE_EXISTS_(in_f);
const string good_f = prefix + std::get<1>(tst);
ASSERT_FILE_EXISTS_(good_f);
my_graph_t graph, graph_good;
graph.loadFromTextFile(in_f);
graph_good.loadFromTextFile(good_f);
ASSERT_(graph.nodeCount() > 1);
ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
// Optimize:
const my_graph_t graph_initial = graph;
mrpt::system::TParametersDouble params;
params["max_iterations"] = 100;
graphslam::TResultInfoSpaLevMarq levmarq_info;
graphslam::optimize_graph_spa_levmarq(
graph, levmarq_info, nullptr, params);
const double err_init = graph_initial.chi2();
const double err_end = graph.chi2();
const double err_good = graph_good.chi2();
/* DEBUG */
#if 1
std::cout << "err_init: " << err_init << std::endl;
std::cout << "err_end: " << err_end << std::endl;
std::cout << "err_good: " << err_good << std::endl;
graph.saveToTextFile("out.graph");
std::cout << "err_init: " << err_init << std::endl;
std::cout << "err_end: " << err_end << std::endl;
std::cout << "err_good: " << err_good << std::endl;
graph.saveToTextFile("out.graph");
#endif
// Do some basic checks on the results:
EXPECT_GE(levmarq_info.num_iters, 10U);
EXPECT_LE(levmarq_info.final_total_sq_error, 5e-2);
EXPECT_LT(err_end, err_init);
// Do some basic checks on the results:
EXPECT_GE(levmarq_info.num_iters, 2U);
EXPECT_LE(levmarq_info.final_total_sq_error, 5e-2);
EXPECT_LT(err_end, err_init);
// Compare to good solution:
compare_two_graphs(graph, graph_good);
// Compare to good solution:
compare_two_graphs(graph, graph_good);
}
}
};
Oops, something went wrong.

0 comments on commit d63818b

Please sign in to comment.