<commit_before>#include <iostream>

#include <rapp-robots-api/localization/localization.hpp>
// search vector std::find
#include <algorithm>
// inverse matrix etc.
#include <eigen3/Eigen/Core>
//#include <geometry_msgs/Pose.h>
//#include <tinyxml2.h>
#include <eigen3/Eigen/Geometry>
// conversions TF to eigen and eigen to TF
//#include <eigen_conversions/eigen_msg.h>
//#include <tf_conversions/tf_eigen.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
namespace rapp {
namespace robot {

// placeholder class for vision implementation
  	localization::localization(int argc, char ** argv)
{
    std::cout << "Initialized placeholder rapp::robot::localization library" << std::endl;
}

    localization::~localization()
{
    std::cout << "Finished placeholder rapp::robot::navigation library" << std::endl;
}
	rapp::object::pose getRobotPoseFromQRcodeMap(rapp::object::qr_code_3d QRcodes,std::vector<std::vector<float>> camera_to_robot_matrix, rapp::object::qr_code_map QRmap){
		rapp::object::pose robot_in_map_pose_RAPP;

		if (QRcodes.number_of_qr_codes > 0){

			// check if one of the detected QRcode is defined in the QRcodeMap

			int k = 0;
			bool QRcode_found_in_map = false;
			int mapped_qrcode_ordinal_nr = -1;
			int found_qrcode_ordinal_nr = -1;
			while ( !QRcode_found_in_map && k < QRcodes.number_of_qr_codes ) {
				std::vector<std::string>::iterator find_iterator;
				find_iterator = std::find(QRmap.labels.begin(), QRmap.labels.end(), QRcodes.qr_message[k]);
				if ( find_iterator != QRmap.labels.end() ){
					QRcode_found_in_map = true;
					found_qrcode_ordinal_nr = k;
					mapped_qrcode_ordinal_nr = std::distance(QRmap.labels.begin(),find_iterator);
				}
				else{
					k += 1;
					QRcode_found_in_map = false;
				}
			}
			//check if any of detected QRcodes is stored in the QRmap
			if (mapped_qrcode_ordinal_nr != -1){

			// compute transformation matrix from map origin to detected QRcode using QRcodeMap
			//geometry_msgs::Pose QRcode_ROS_pose;
			//Eigen::Affine3f map_to_QRcode_transform; 


			// // ... NEW APPROACH

			Eigen::Quaternion<float> map_to_qr_code_quaternion(QRmap.poses[mapped_qrcode_ordinal_nr].orientation.w, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.x, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.y, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.z);
			//Eigen::Matrix3d rotationMatrix = q.matrix();

			Eigen::Translation<float,3> map_to_qr_code_translation;
			Eigen::Transform<float,3,Eigen::Affine>  map_to_QRcode_transform= Eigen::Transform<float,3,Eigen::Affine>::Identity();
			map_to_QRcode_transform = map_to_qr_code_quaternion*map_to_qr_code_translation;
			//map_to_QRcode_transform.rotate(map_to_qr_code_quaternion);
			/*
			 tf::Quaternion q(QR

map.poses[mapped_qrcode_ordinal_nr].orientation.x,QRmap.poses[mapped_qrcode_ordinal_nr].orientation.y, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.z, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.w);
			 tf::Matrix3x3 m(q);
			 double roll, pitch, yaw;
			 m.getRPY(roll, pitch, yaw);
			 // std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
			std::vector<std::vector<double>> T_g_qr = computeMatricFromPose(QRmap.poses[mapped_qrcode_ordinal_nr].position.x,QRmap.poses[mapped_qrcode_ordinal_nr].position.y,
			 																		QRmap.poses[mapped_qrcode_ordinal_nr].position.z, roll, pitch, yaw);




			map_to_QRcode_transform.matrix() << T_g_qr[0][0], T_g_qr[0][1], T_g_qr[0][2], T_g_qr[0][3],
												T_g_qr[1][0], T_g_qr[1][1], T_g_qr[1][2], T_g_qr[1][3],
												T_g_qr[2][0], T_g_qr[2][1], T_g_qr[2][2], T_g_qr[2][3],
												T_g_qr[3][0], T_g_qr[3][1], T_g_qr[3][2], T_g_qr[3][3];
<commit_msg>fixes QR translation with respect to map<commit_after>#include <iostream>

#include <rapp-robots-api/localization/localization.hpp>
// search vector std::find
#include <algorithm>
// inverse matrix etc.
#include <eigen3/Eigen/Core>
//#include <geometry_msgs/Pose.h>
//#include <tinyxml2.h>
#include <eigen3/Eigen/Geometry>
// conversions TF to eigen and eigen to TF
//#include <eigen_conversions/eigen_msg.h>
//#include <tf_conversions/tf_eigen.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>

namespace rapp {
namespace robot {

// placeholder class for vision implementation
  	localization::localization(int argc, char ** argv)
{
    std::cout << "Initialized placeholder rapp::robot::localization library" << std::endl;
}

    localization::~localization()
{
    std::cout << "Finished placeholder rapp::robot::navigation library" << std::endl;
}
	rapp::object::pose getRobotPoseFromQRcodeMap(rapp::object::qr_code_3d QRcodes,std::vector<std::vector<float>> camera_to_robot_matrix, rapp::object::qr_code_map QRmap){
		rapp::object::pose robot_in_map_pose_RAPP;

		if (QRcodes.number_of_qr_codes > 0){

			// check if one of the detected QRcode is defined in the QRcodeMap

			int k = 0;
			bool QRcode_found_in_map = false;
			int mapped_qrcode_ordinal_nr = -1;
			int found_qrcode_ordinal_nr = -1;
			while ( !QRcode_found_in_map && k < QRcodes.number_of_qr_codes ) {
				std::vector<std::string>::iterator find_iterator;
				find_iterator = std::find(QRmap.labels.begin(), QRmap.labels.end(), QRcodes.qr_message[k]);
				if ( find_iterator != QRmap.labels.end() ){
					QRcode_found_in_map = true;
					found_qrcode_ordinal_nr = k;
					mapped_qrcode_ordinal_nr = std::distance(QRmap.labels.begin(),find_iterator);
				}
				else{
					k += 1;
					QRcode_found_in_map = false;
				}
			}
			//check if any of detected QRcodes is stored in the QRmap
			if (mapped_qrcode_ordinal_nr != -1){
			// compute transformation matrix from map origin to detected QRcode using QRcodeMap
			//geometry_msgs::Pose QRcode_ROS_pose;
			//Eigen::Affine3f map_to_QRcode_transform; 


			// // ... NEW APPROACH

			Eigen::Quaternion<float> map_to_qr_code_quaternion(QRmap.poses[mapped_qrcode_ordinal_nr].orientation.w, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.x, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.y, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.z);
			//Eigen::Matrix3d rotationMatrix = q.matrix();

			Eigen::Translation<float,3> map_to_qr_code_translation;
			map_to_qr_code_translation.x() = QRmap.poses[mapped_qrcode_ordinal_nr].position.x;
			map_to_qr_code_translation.y() = QRmap.poses[mapped_qrcode_ordinal_nr].position.y;
			map_to_qr_code_translation.z() = QRmap.poses[mapped_qrcode_ordinal_nr].position.z;

			Eigen::Transform<float,3,Eigen::Affine>  map_to_QRcode_transform= Eigen::Transform<float,3,Eigen::Affine>::Identity();
			map_to_QRcode_transform = map_to_qr_code_quaternion*map_to_qr_code_translation;

			//map_to_QRcode_transform.rotate(map_to_qr_code_quaternion);
			/*
			 tf::Quaternion q(QR

map.poses[mapped_qrcode_ordinal_nr].orientation.x,QRmap.poses[mapped_qrcode_ordinal_nr].orientation.y, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.z, QRmap.poses[mapped_qrcode_ordinal_nr].orientation.w);
			 tf::Matrix3x3 m(q);
			 double roll, pitch, yaw;
			 m.getRPY(roll, pitch, yaw);
			 // std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
			std::vector<std::vector<double>> T_g_qr = computeMatricFromPose(QRmap.poses[mapped_qrcode_ordinal_nr].position.x,QRmap.poses[mapped_qrcode_ordinal_nr].position.y,
			 																		QRmap.poses[mapped_qrcode_ordinal_nr].position.z, roll, pitch, yaw);




			map_to_QRcode_transform.matrix() << T_g_qr[0][0], T_g_qr[0][1], T_g_qr[0][2], T_g_qr[0][3],
												T_g_qr[1][0], T_g_qr[1][1], T_g_qr[1][2], T_g_qr[1][3],
												T_g_qr[2][0], T_g_qr[2][1], T_g_qr[2][2], T_g_qr[2][3],
												T_g_qr[3][0], T_g_qr[3][1], T_g_qr[3][2], T_g_qr[3][3];

{'fraction_non_alphanumeric': 0.1422551398288852, 'fraction_numerical': 0.012514365981356149, 'mean_word_length': 5.085470085470085, 'pattern_counts': {'":': 0, '<': 95, '<?xml version=': 0, '>': 47, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 4}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1195409', 'n_tokens_mistral': 3471, 'n_tokens_neox': 2953, 'n_words': 444}