<commit_before>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include <mrpt/random.h>
#include <mrpt/graphslam.h>
#include <mrpt/system/threads.h> // sleep()
#include <mrpt/gui.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/opengl/graph_tools.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::graphs;
using namespace mrpt::graphslam;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::random;
using namespace std;


// Level of noise in nodes initial positions:
const double STD_NOISE_NODE_XYZ = 0.5;
const double STD_NOISE_NODE_ANG = DEG2RAD(5);

// Level of noise in edges:
const double STD_NOISE_EDGE_XYZ = 0.001;
const double STD_NOISE_EDGE_ANG = DEG2RAD(0.01);

const double STD4EDGES_COV_MATRIX       = 10;
const double ERROR_IN_INCOMPATIBLE_EDGE = 0.3; // ratio [0,1]


// Auxiliary class to add a new edge to the graph. The edge is annotated with the relative position of the two nodes
template <class GRAPH,bool EDGES_ARE_PDF = GRAPH::edge_t::is_PDF_val> struct EdgeAdders;

// Non-PDF version:
template <class GRAPH> struct EdgeAdders<GRAPH,false>
{
	static const int DIM = GRAPH::edge_t::type_value::static_size;
	typedef CMatrixFixedNumeric<double,DIM,DIM> cov_t;

	static void addEdge(TNodeID from, TNodeID to, const typename GRAPH::global_poses_t &real_poses,GRAPH &graph, const cov_t &COV_MAT)
	{
		typename GRAPH::edge_t RelativePose(real_poses.find(to)->second - real_poses.find(from)->second);
		graph.insertEdge(from,to, RelativePose );
	}
};
// PDF version:
template <class GRAPH> struct EdgeAdders<GRAPH,true>
{
	static const int DIM = GRAPH::edge_t::type_value::static_size;
	typedef CMatrixFixedNumeric<double,DIM,DIM> cov_t;

	static void addEdge(TNodeID from, TNodeID to, const typename GRAPH::global_poses_t &real_poses,GRAPH &graph, const cov_t &COV_MAT)
	{
		typename GRAPH::edge_t RelativePose(
			real_poses.find(to)->second - real_poses.find(from)->second,
			COV_MAT);
		graph.insertEdge(from,to, RelativePose );
	}
};

vector<double>  log_sq_err_evolution;

// This example lives inside this template class, which can be instanced for different kind of graphs (see main()):
template <class my_graph_t>
struct ExampleDemoGraphSLAM
{
	template <class GRAPH_T>
	static void my_levmarq_feedback(
		const GRAPH_T &graph,
		const size_t iter,
		const size_t max_iter,
		const double cur_sq_error )
	{
		log_sq_err_evolution.push_back(std::log(cur_sq_error));
		if ((iter % 100)==0)
			cout << "Progress: " << iter << " / " << max_iter << ", total sq err = " << cur_sq_error << endl;
	}

	// ------------------------------------------------------
	//				GraphSLAMDemo
	// ------------------------------------------------------
	void run(bool add_extra_tightening_edge)
	{
		// The graph: nodes + edges:
		my_graph_t  graph;

		// The global poses of each node (without covariance):
		typename my_graph_t::global_poses_t  real_node_poses;

		randomGenerator.randomize(123);

		// ----------------------------
		// Create a random graph:
		// ----------------------------
		const size_t N_VERTEX = 50;
		const double DIST_THRES = 7;
		const double NODES_XY_MAX = 20;


		for (TNodeID j=0;j<N_VERTEX;j++)
		{
			// Use evenly distributed nodes:
			static double ang = 2*M_PI/N_VERTEX;
			const double R = NODES_XY_MAX + 2 * (j % 2 ? 1:-1);
			CPose2D p(
				R*cos(ang*j),
				R*sin(ang*j),
				ang);

			// Save real pose:
			real_node_poses[j] = p;

			// Copy the nodes to the graph, and add some noise:
			graph.nodes[j] = p;
		}


		// Add some edges
		typedef EdgeAdders<my_graph_t> edge_adder_t;
		typename edge_adder_t::cov_t   inf_matrix;
		inf_matrix.unit(edge_adder_t::cov_t::RowsAtCompileTime, square(1.0/STD4EDGES_COV_MATRIX));

		for (TNodeID i=0;i<N_VERTEX;i++)
		{
			for (TNodeID j=i+1;j<N_VERTEX;j++)
			{
				if ( real_node_poses[i].distanceTo(real_node_poses[j]) < DIST_THRES )
					edge_adder_t::addEdge(i,j,real_node_poses,graph,inf_matrix);
			}
		}

		// Add an additional edge to deforme the graph?
		if (add_extra_tightening_edge)
		{
			//inf_matrix.unit(square(1.0/(STD4EDGES_COV_MATRIX)));
			edge_adder_t::addEdge(0,N_VERTEX/2,real_node_poses,graph,inf_matrix);

			// Tweak this last node to make it incompatible with the rest:
			typename my_graph_t::edge_t &ed = graph.edges.find(make_pair<TNodeID,TNodeID>(0,N_VERTEX/2))->second; // It must exist, don't check errors...
			ed.getPoseMean().x( (1-ERROR_IN_INCOMPATIBLE_EDGE) * ed.getPoseMean().x() );
		}

		// The root node (the origin of coordinates):
		graph.root = TNodeID(0);

		// This is the ground truth graph (make a copy for later use):
		const my_graph_t  graph_GT = graph;

		cout << "graph nodes: " << graph_GT.nodeCount() << endl;<commit_msg>Documented parts of graph_slam_demo sample<commit_after>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include <mrpt/random.h>
#include <mrpt/graphslam.h>
#include <mrpt/system/threads.h> // sleep()
#include <mrpt/gui.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/opengl/graph_tools.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::graphs;
using namespace mrpt::graphslam;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::random;
using namespace std;


// Level of noise in nodes initial positions:
const double STD_NOISE_NODE_XYZ = 0.5;
const double STD_NOISE_NODE_ANG = DEG2RAD(5);

// Level of noise in edges:
const double STD_NOISE_EDGE_XYZ = 0.001;
const double STD_NOISE_EDGE_ANG = DEG2RAD(0.01);

const double STD4EDGES_COV_MATRIX       = 10;
const double ERROR_IN_INCOMPATIBLE_EDGE = 0.3; // ratio [0,1]


/** 
 * Generic struct template
 * Auxiliary class to add a new edge to the graph. The edge is annotated with the relative position of the two nodes
 */
template <class GRAPH,bool EDGES_ARE_PDF = GRAPH::edge_t::is_PDF_val> struct EdgeAdders;

/**
 * Specific templates based on the above EdgeAdders temptlate
 * Non-PDF version:
 */
template <class GRAPH> struct EdgeAdders<GRAPH,false>
{
	static const int DIM = GRAPH::edge_t::type_value::static_size;
	typedef CMatrixFixedNumeric<double,DIM,DIM> cov_t;

	static void addEdge(TNodeID from, TNodeID to, const typename GRAPH::global_poses_t &real_poses,GRAPH &graph, const cov_t &COV_MAT)
	{
		typename GRAPH::edge_t RelativePose(real_poses.find(to)->second - real_poses.find(from)->second);
		graph.insertEdge(from,to, RelativePose );
	}
};
// PDF version:
template <class GRAPH> struct EdgeAdders<GRAPH,true>
{
	static const int DIM = GRAPH::edge_t::type_value::static_size;
	typedef CMatrixFixedNumeric<double,DIM,DIM> cov_t;

	static void addEdge(TNodeID from, TNodeID to, const typename GRAPH::global_poses_t &real_poses,GRAPH &graph, const cov_t &COV_MAT)
	{
		typename GRAPH::edge_t RelativePose(
			real_poses.find(to)->second - real_poses.find(from)->second,
			COV_MAT);
		graph.insertEdge(from,to, RelativePose );
	}
};

// Container to handle the propagation of the square root error of the problem
vector<double>  log_sq_err_evolution;

// This example lives inside this template class, which can be instanced for different kind of graphs (see main()):
template <class my_graph_t>
struct ExampleDemoGraphSLAM
{
	template <class GRAPH_T>
	static void my_levmarq_feedback(
		const GRAPH_T &graph,
		const size_t iter,
		const size_t max_iter,
		const double cur_sq_error )
	{
		log_sq_err_evolution.push_back(std::log(cur_sq_error));
		if ((iter % 100)==0)
			cout << "Progress: " << iter << " / " << max_iter << ", total sq err = " << cur_sq_error << endl;
	}

	// ------------------------------------------------------
	//				GraphSLAMDemo
	// ------------------------------------------------------
	void run(bool add_extra_tightening_edge)
	{
		// The graph: nodes + edges:
		my_graph_t  graph;

		// The global poses of the graph nodes (without covariance):
		typename my_graph_t::global_poses_t  real_node_poses;

    /**
     * Initialize the PRNG from the given random seed.
     * Method used to initially randomise the generator 
     */
		randomGenerator.randomize(123);

		// ----------------------------
		// Create a random graph:
		// ----------------------------
		const size_t N_VERTEX = 50;
		const double DIST_THRES = 7;
		const double NODES_XY_MAX = 20;


		for (TNodeID j=0;j<N_VERTEX;j++)
		{
			// Use evenly distributed nodes:
			static double ang = 2*M_PI/N_VERTEX;
			const double R = NODES_XY_MAX + 2 * (j % 2 ? 1:-1);
			CPose2D p(
				R*cos(ang*j),
				R*sin(ang*j),
				ang);

			// Save real pose:
			real_node_poses[j] = p;

			// Copy the nodes to the graph, and add some noise:
			graph.nodes[j] = p;
		}


		// Add some edges
		typedef EdgeAdders<my_graph_t> edge_adder_t;
		typename edge_adder_t::cov_t   inf_matrix;
		inf_matrix.unit(edge_adder_t::cov_t::RowsAtCompileTime, square(1.0/STD4EDGES_COV_MATRIX));

		for (TNodeID i=0;i<N_VERTEX;i++)
		{
			for (TNodeID j=i+1;j<N_VERTEX;j++)
			{
				if ( real_node_poses[i].distanceTo(real_node_poses[j]) < DIST_THRES )
					edge_adder_t::addEdge(i,j,real_node_poses,graph,inf_matrix);
			}
		}

		// Add an additional edge to deforme the graph?
		if (add_extra_tightening_edge)
		{
			//inf_matrix.unit(square(1.0/(STD4EDGES_COV_MATRIX)));
			edge_adder_t::addEdge(0,N_VERTEX/2,real_node_poses,graph,inf_matrix);

			// Tweak this last node to make it incompatible with the rest
      // It must exist, don't check errors...
			typename my_graph_t::edge_t &ed = graph.edges.find(make_pair<TNodeID,TNodeID>(0,N_VERTEX/2))->second; 
			ed.getPoseMean().x( (1-ERROR_IN_INCOMPATIBLE_EDGE) * ed.getPoseMean().x() );
		}

		// The root node (the origin of coordinates):
		graph.root = TNodeID(0);

		// This is the ground truth graph (make a copy for later use):
		const my_graph_t  graph_GT = graph;

		cout << "graph nodes: " << graph_GT.nodeCount() << endl;
{'fraction_non_alphanumeric': 0.16639900071377586, 'fraction_numerical': 0.010706638115631691, 'mean_word_length': 2.7855454238432964, 'pattern_counts': {'":': 0, '<': 89, '<?xml version=': 0, '>': 51, 'https://': 0, 'lorem ipsum': 0, 'www.': 6, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7637669', 'n_tokens_mistral': 4216, 'n_tokens_neox': 3781, 'n_words': 911}