Skip to content

Commit

Permalink
fix monumental error in Hessian calculation
Browse files Browse the repository at this point in the history 
cc #770
  • Loading branch information
@jlblancoc
jlblancoc committed May 24, 2018
1 parent 8786bd6 commit d63818b
Show file tree
Hide file tree
Showing 5 changed files with 227 additions and 92 deletions.
23 changes: 10 additions & 13 deletions libs/graphslam/include/mrpt/graphslam/levmarq.h
View file
Expand Up @@ -52,7 +52,7 @@ namespace mrpt::graphslam
*iterations. *iterations.
* - "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial * - "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial
*lambda value for the lev-marq algorithm. *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 * - "e1": (default=1e-6) Lev-marq algorithm iteration stopping criterion
*#1: *#1:
*|gradient| < e1 *|gradient| < e1
Expand Down Expand Up @@ -245,7 +245,6 @@ void optimize_graph_spa_levmarq(
grad.setZero(); grad.setZero();
using map_ID2matrix_VxV_t = using map_ID2matrix_VxV_t =
mrpt::aligned_std_map<TNodeID, typename gst::matrix_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 lambda = initial_lambda; // Will be actually set on first iteration.
double v = 1; // was 2, changed since it's modified in the first pass. double v = 1; // was 2, changed since it's modified in the first pass.
Expand All @@ -260,6 +259,7 @@ void optimize_graph_spa_levmarq(


for (size_t iter = 0; iter < max_iters; ++iter) for (size_t iter = 0; iter < max_iters; ++iter)
{ {
vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
last_iter = iter; last_iter = iter;


// This will be false only when the delta leads to a worst solution and // This will be false only when the delta leads to a worst solution and
Expand Down Expand Up @@ -411,7 +411,7 @@ void optimize_graph_spa_levmarq(
J1, J2, JtJ, lstObservationData[idxObs].edge); J1, J2, JtJ, lstObservationData[idxObs].edge);
// We sort IDs such as "i" < "j" and we can build just // We sort IDs such as "i" < "j" and we can build just
// the upper triangular part of the Hessian: // 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; H_map[idx_j][idx_i] += JtJ;
else else
H_map[idx_i][idx_j] += JtJ.transpose(); H_map[idx_i][idx_j] += JtJ.transpose();
Expand Down Expand Up @@ -499,9 +499,11 @@ void optimize_graph_spa_levmarq(
it->second.get_unsafe(r, r) + lambda); it->second.get_unsafe(r, r) + lambda);
// c>r: // c>r:
for (size_t c = r + 1; c < DIMS_POSE; c++) for (size_t c = r + 1; c < DIMS_POSE; c++)
{
sp_H.insert_entry_fast( sp_H.insert_entry_fast(
j_offset + r, i_offset + c, j_offset + r, i_offset + c,
it->second.get_unsafe(r, c)); it->second.get_unsafe(r, c));
}
} }
} }
else else
Expand Down Expand Up @@ -602,24 +604,19 @@ void optimize_graph_spa_levmarq(
nodes_to_optimize->begin(); nodes_to_optimize->begin();
it != nodes_to_optimize->end(); ++it) 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; typename gst::Array_O exp_delta;
for (size_t i = 0; i < DIMS_POSE; i++) for (size_t i = 0; i < DIMS_POSE; i++)
exp_delta[i] = -*delta_ptr++; // The "-" sign is for exp_delta[i] = -*delta_ptr++;
// the missing "-" // The "-" above is for the missing "-" dragged from the
// carried all this time // Gauss-Newton formula above.
// from above
gst::SE_TYPE::exp(exp_delta, exp_delta_pose);


// new_x_i = exp_delta_i (+) old_x_i // new_x_i = exp_delta_i (+) old_x_i
typename gst::graph_t::global_poses_t::iterator typename gst::graph_t::global_poses_t::iterator
it_old_value = graph.nodes.find(*it); it_old_value = graph.nodes.find(*it);
old_poses_backup[*it] = old_poses_backup[*it] =
it_old_value->second; // back up the old pose as a copy it_old_value->second; // back up the old pose as a copy
it_old_value->second.composeFrom( detail::AuxPoseOPlus<typename gst::edge_t, gst>::sumIncr(
exp_delta_pose, it_old_value->second); it_old_value->second, exp_delta);
} }
} }


Expand Down
105 changes: 80 additions & 25 deletions libs/graphslam/include/mrpt/graphslam/levmarq_impl.h
View file
Expand Up @@ -22,6 +22,67 @@ using namespace mrpt::graphslam;
using namespace mrpt::math; using namespace mrpt::math;
using namespace std; 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 // An auxiliary struct to compute the pseudo-ln of a pose error, possibly
// modified with an information matrix. // modified with an information matrix.
// Specializations are below. // Specializations are below.
Expand All @@ -34,18 +95,18 @@ struct AuxErrorEval<CPose2D, gst>
{ {
template <class POSE, class VEC, class EDGE_ITERATOR> template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError( 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); MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err); gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
} }


template <class MAT, class EDGE_ITERATOR> template <class MAT, class EDGE_ITERATOR>
static inline void multiplyJtLambdaJ( static inline void multiplyJtLambdaJ(
const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge) const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
{ {
MRPT_UNUSED_PARAM(edge); MRPT_UNUSED_PARAM(edge);
JtJ = J1.transpose()*J1; JtJ = J1.transpose() * J1;
} }


template <class MAT, class EDGE_ITERATOR> template <class MAT, class EDGE_ITERATOR>
Expand All @@ -61,7 +122,8 @@ struct AuxErrorEval<CPose2D, gst>
const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT) const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
{ {
MRPT_UNUSED_PARAM(edge); MRPT_UNUSED_PARAM(edge);
OUT += J.transpose() * ERR; const auto grad_incr = (J.transpose() * ERR).eval();
OUT += grad_incr;
} }
}; };


Expand All @@ -71,10 +133,10 @@ struct AuxErrorEval<CPose3D, gst>
{ {
template <class POSE, class VEC, class EDGE_ITERATOR> template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError( 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); MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err); gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
} }


template <class MAT, class EDGE_ITERATOR> template <class MAT, class EDGE_ITERATOR>
Expand Down Expand Up @@ -108,9 +170,9 @@ struct AuxErrorEval<CPosePDFGaussianInf, gst>
{ {
template <class POSE, class VEC, class EDGE_ITERATOR> template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError( 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> template <class MAT, class EDGE_ITERATOR>
Expand Down Expand Up @@ -140,10 +202,10 @@ struct AuxErrorEval<CPose3DPDFGaussianInf, gst>
{ {
template <class POSE, class VEC, class EDGE_ITERATOR> template <class POSE, class VEC, class EDGE_ITERATOR>
static inline void computePseudoLnError( 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); MRPT_UNUSED_PARAM(edge);
gst::SE_TYPE::pseudo_ln(P1DP2inv, err); gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
} }


template <class MAT, class EDGE_ITERATOR> template <class MAT, class EDGE_ITERATOR>
Expand Down Expand Up @@ -201,31 +263,24 @@ double computeJacobiansAndErrors(
const auto& edge = e->second; const auto& edge = e->second;


// Compute the residual pose error of these pair of nodes + its // Compute the residual pose error of these pair of nodes + its
// constraint, // constraint:
// that is: P1DP2inv = P1 * EDGE * inv(P2) // DinvP1invP2 = inv(EDGE) * inv(P1) * P2 = (P2 \ominus P1) \ominus EDGE
typename gst::graph_t::constraint_t::type_value P1DP2inv( typename gst::graph_t::constraint_t::type_value DinvP1invP2 =
mrpt::poses::UNINITIALIZED_POSE); ((*P2) - (*P1)) - *EDGE_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);
}


// Add to vector of errors: // Add to vector of errors:
errs.resize(errs.size() + 1); errs.resize(errs.size() + 1);
detail::AuxErrorEval<typename gst::edge_t, gst>::computePseudoLnError( detail::AuxErrorEval<typename gst::edge_t, gst>::computePseudoLnError(
P1DP2inv, errs.back(), edge); DinvP1invP2, errs.back(), edge);


// Compute the jacobians: // Compute the jacobians:
alignas(MRPT_MAX_ALIGN_BYTES) alignas(MRPT_MAX_ALIGN_BYTES)
std::pair<mrpt::graphs::TPairNodeIDs, typename gst::TPairJacobs> std::pair<mrpt::graphs::TPairNodeIDs, typename gst::TPairJacobs>
newMapEntry; newMapEntry;
newMapEntry.first = ids; newMapEntry.first = ids;
gst::SE_TYPE::jacobian_dP1DP2inv_depsilon( gst::SE_TYPE::jacobian_dDinvP1invP2_depsilon(
P1DP2inv, &newMapEntry.second.first, &newMapEntry.second.second); -(*EDGE_POSE), *P1, *P2, &newMapEntry.second.first,
&newMapEntry.second.second);


// And insert into map of jacobians: // And insert into map of jacobians:
lstJacobians.insert(lstJacobians.end(), newMapEntry); lstJacobians.insert(lstJacobians.end(), newMapEntry);
Expand Down
98 changes: 53 additions & 45 deletions libs/graphslam/src/graph_slam_levmarq_unittest.cpp
View file
Expand Up @@ -27,12 +27,15 @@ using namespace mrpt::math;
using namespace std; using namespace std;


// Define in/out files for testing: // 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{ const std::map<std::string, in_out_filenames> inout_graph_files{
{"GraphTester2D", {"GraphTester2D",
{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"}}, {{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"}}},
{"GraphTester2DInf", {"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> template <class my_graph_t>
class GraphTester : public GraphSlamLevMarqTest<my_graph_t>, class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
Expand All @@ -53,7 +56,6 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
// Run graph slam: // Run graph slam:
// ---------------------------- // ----------------------------
mrpt::system::TParametersDouble params; mrpt::system::TParametersDouble params;
// params["verbose"] = 1;
params["max_iterations"] = 100; params["max_iterations"] = 100;


graphslam::TResultInfoSpaLevMarq levmarq_info; graphslam::TResultInfoSpaLevMarq levmarq_info;
Expand All @@ -65,7 +67,7 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
const double err_end = graph.chi2(); const double err_end = graph.chi2();


// Do some basic checks on the results: // 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_LE(levmarq_info.final_total_sq_error, 5e-2);
EXPECT_LT(err_end, err_init); EXPECT_LT(err_end, err_init);


Expand Down Expand Up @@ -115,9 +117,10 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
.abs() .abs()
.maxCoeff(), .maxCoeff(),
eps_node_pos) eps_node_pos)
<< "Poses of keyframe #" << itn1->first << " do not match:" << std::endl << "Poses of keyframe #" << itn1->first
<< "- Expected: " << itn2->second << " do not match:" << std::endl
<< std::endl << "- Got : " << itn1->second << std::endl; << "- Expected: " << itn2->second << std::endl
<< "- Got : " << itn1->second << std::endl;
} }
} }
} }
Expand Down Expand Up @@ -160,46 +163,51 @@ class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
return; // No tests for this type return; // No tests for this type


const string prefix = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/"); const string prefix = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/");
const string in_f = prefix + std::get<0>(files_it->second); for (const auto& tst : files_it->second)
ASSERT_FILE_EXISTS_(in_f); {
const string good_f = prefix + std::get<1>(files_it->second); std::cout << "Testing graph type `" << type << "`, in_file=`"
ASSERT_FILE_EXISTS_(good_f); << std::get<0>(tst) << "`" << std::endl;


my_graph_t graph, graph_good; const string in_f = prefix + std::get<0>(tst);
graph.loadFromTextFile(in_f); ASSERT_FILE_EXISTS_(in_f);
graph_good.loadFromTextFile(good_f); const string good_f = prefix + std::get<1>(tst);
ASSERT_(graph.nodeCount() > 1); ASSERT_FILE_EXISTS_(good_f);
ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount()); my_graph_t graph, graph_good;

graph.loadFromTextFile(in_f);
// Optimize: graph_good.loadFromTextFile(good_f);
const my_graph_t graph_initial = graph; ASSERT_(graph.nodeCount() > 1);
mrpt::system::TParametersDouble params; ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
// params["verbose"] = 1; ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
params["max_iterations"] = 100;

// Optimize:
graphslam::TResultInfoSpaLevMarq levmarq_info; const my_graph_t graph_initial = graph;

mrpt::system::TParametersDouble params;
graphslam::optimize_graph_spa_levmarq( params["max_iterations"] = 100;
graph, levmarq_info, nullptr, params);

graphslam::TResultInfoSpaLevMarq levmarq_info;
const double err_init = graph_initial.chi2();
const double err_end = graph.chi2(); graphslam::optimize_graph_spa_levmarq(
const double err_good = graph_good.chi2(); graph, levmarq_info, nullptr, params);
/* DEBUG */
const double err_init = graph_initial.chi2();
const double err_end = graph.chi2();
const double err_good = graph_good.chi2();
/* DEBUG */
#if 1 #if 1
std::cout << "err_init: " << err_init << std::endl; std::cout << "err_init: " << err_init << std::endl;
std::cout << "err_end: " << err_end << std::endl; std::cout << "err_end: " << err_end << std::endl;
std::cout << "err_good: " << err_good << std::endl; std::cout << "err_good: " << err_good << std::endl;
graph.saveToTextFile("out.graph"); graph.saveToTextFile("out.graph");
#endif #endif
// Do some basic checks on the results: // 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_LE(levmarq_info.final_total_sq_error, 5e-2);
EXPECT_LT(err_end, err_init); EXPECT_LT(err_end, err_init);


// Compare to good solution: // Compare to good solution:
compare_two_graphs(graph, graph_good); compare_two_graphs(graph, graph_good);
}
} }
}; };


Expand Down

0 comments on commit d63818b

Please sign in to comment.