#include <cstdlib>
#include <cstdio>
#include <math.h>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <utility>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions>
Go to the source code of this file.
|
Eigen::Quaterniond | addQ (Eigen::Quaterniond a, Eigen::Quaterniond b) |
|
Matrix4d | calc_RT (MatrixXd lidar, MatrixXd camera, int MAX_ITERS, Eigen::Matrix3d lidarToCamera) |
|
void | find_transformation (std::vector< float > marker_info, int num_of_marker_in_config, int MAX_ITERS, Eigen::Matrix3d lidarToCamera) |
|
std::pair< MatrixXd, MatrixXd > | readArray () |
|
void | readArucoPose (std::vector< float > marker_info, int num_of_marker_in_config) |
|
Eigen::Quaterniond addQ |
( |
Eigen::Quaterniond |
a, |
|
|
Eigen::Quaterniond |
b |
|
) |
| |
Matrix4d calc_RT |
( |
MatrixXd |
lidar, |
|
|
MatrixXd |
camera, |
|
|
int |
MAX_ITERS, |
|
|
Eigen::Matrix3d |
lidarToCamera |
|
) |
| |
void find_transformation |
( |
std::vector< float > |
marker_info, |
|
|
int |
num_of_marker_in_config, |
|
|
int |
MAX_ITERS, |
|
|
Eigen::Matrix3d |
lidarToCamera |
|
) |
| |
std::pair<MatrixXd, MatrixXd> readArray |
( |
| ) |
|
void readArucoPose |
( |
std::vector< float > |
marker_info, |
|
|
int |
num_of_marker_in_config |
|
) |
| |
int iteration_counter = 0 |
Eigen::Matrix3d rotation_avg_by_mult |
Eigen::Quaterniond rotation_sum |
Eigen::Vector3d translation_sum |