int main(int argc, char* argv[]) {
if (argc != 2) {
std::cout << "Need filename" << std::endl;
return -1;
}
char* filename = argv[1];
std::vector<std::string*>* strReps = readlines(filename);
std::vector<std::vector<int>*>* vectorReps = map(strReps, &transform);
std::vector<int>* vectorRes = sum(vectorReps);
std::string* strRes = transform_1(vectorRes);
std::cout << *strRes << std::endl;
assert((*strRes).substr(0, 10) == std::string("5537376230"));
for (int i = 0; i < strReps->size(); i++) {
delete strReps->at(i);
}
delete strReps;
for (int i = 0; i < vectorReps->size(); i++) {
delete vectorReps->at(i);
}
delete vectorReps;
delete vectorRes;
delete strRes;
return 0;
}
SPROUT_CONSTEXPR position_type
transform(position_type const& c, unit_type const& u, unit_type const& v) const {
return transform_1(
c,
u * sprout::cos(rotate_) - v * sprout::sin(rotate_),
u * sprout::sin(rotate_) + v * sprout::cos(rotate_),
sprout::sqrt(
sprout::darkroom::coords::x(c) * sprout::darkroom::coords::x(c)
+ sprout::darkroom::coords::z(c) * sprout::darkroom::coords::z(c)
)
);
}
// This is the main function
int
main (int argc, char** argv)
{
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}
// Fetch point cloud filename in arguments | Works with PCD and PLY files
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
if (filenames.size () != 1) {
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 1) {
showHelp (argv[0]);
return -1;
} else {
file_is_pcd = true;
}
}
// Load file | Works with PCD and PLY files
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (file_is_pcd) {
if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
} else {
if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
}
/* Reminder: how transformation matrices work :
|-------> This column is the translation
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left
| 0 0 1 z | /
| 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1)
METHOD #1: Using a Matrix4f
This is the "manual" method, perfect to understand but error prone !
*/
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI/4; // The angle of rotation in radians
transform_1 (0,0) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
// (row, column)
// Define a translation of 2.5 meters on the x axis.
transform_1 (0,3) = 2.5;
// Print the transformation
printf ("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl;
/* METHOD #2: Using a Affine3f
This method is easier and less error prone
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
printf ("\nMethod #2: using an Affine3f\n");
std::cout << transform_2.matrix() << std::endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
// Visualization
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, "cloud", 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return 0;
}
// I need to change the parameters to account for the
// transformation matrices. I need to save the best TM
// so I can combine it with the icp TM.
//
// Does kdtree search on each rotation of 45 degrees around
// the central point of the moved_cloud
Eigen::Matrix4f kdtree_search(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud, float radius)
{
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// Total number of nearest neighbors
int num_matches = 0;
// Return the cloud with the most number of nearest neighbors
// within a given radius of the searchPoint
int max_neighbors = 0;
pcl::PointCloud<pcl::PointXYZ>::Ptr best_fit_cloud ;
Eigen::Matrix4f best_fit_transform = Eigen::Matrix4f::Identity();
Eigen::Vector4f centroid1 = compute_centroid(*moved_cloud);
cout << "centroid of source cloud - " << centroid1(0)
<< ", " << centroid1(1) << ", " << centroid1(2) << endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity();
for (int i = 0; i < 8; i++)
{
float theta = 45 * i + 45;
transform_1 (0,0) = cos (theta);
transform_1 (0,2) = -sin (theta);
transform_1 (2,0) = sin (theta);
transform_1 (2,2) = cos (theta);
cout << "cloud with " << theta << " degrees of rotation" << endl;
pcl::transformPointCloud (*moved_cloud, *rotated_cloud, transform_1);
// Probably need to compute centroid of the new transformed cloud
// because the transformation seems to translate it
Eigen::Vector4f centroid2 = compute_centroid(*rotated_cloud);
cout << "centroid of rotated cloud - " << centroid2(0)
<< ", " << centroid2(1) << ", " << centroid2(2) << endl;
float distance_x = centroid1(0) - centroid2(0);
float distance_y = centroid1(1) - centroid2(1);
float distance_z = centroid1(2) - centroid2(2);
cout << "distance between centroids: (" << distance_x << ", " << distance_y << ", " << distance_z << ")" << endl;
transform_2 (0,3) = (distance_x);
transform_2 (1,3) = (distance_y);
transform_2 (2,3) = (distance_z);
pcl::transformPointCloud (*rotated_cloud, *transformed_cloud, transform_2);
// Rotate the cloud by 45 degrees each time
// May want to add some random translation as well.
// This would correspond to doing kdtree search on a number of
// clouds that are presumably close to the target point cloud.
kdtree.setInputCloud(transformed_cloud);
// Run the kdtree search on every 10th point of the moved_cloud
// Increase this number to speed up the search
// Test with different increments of i to see effect on speed
for (int j = 0; j < (*moved_cloud).points.size(); j += 10)
{
pcl::PointXYZ searchPoint = moved_cloud->points[j];
int num_neighbors = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
num_matches += num_neighbors;
cout << "performed kdtree nearest neighbor search, found " << num_neighbors << " within " << radius << " radius" << endl;
}
cout << "num_matches = " << num_matches << endl;
cout << "max_neighbors = " << max_neighbors << endl;
if (num_matches > max_neighbors) {
max_neighbors = num_matches;
best_fit_cloud = transformed_cloud;
// are these transforms relative? or absolute?
// this currently calculates relative
// should be transform_2 * transform_1 if absolute
best_fit_transform = transform_2 * transform_1;
}
num_matches = 0;
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n"
" blue = moved point cloud\n"
" green = rotated point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white
// We add the point cloud to the viewer and pass the color handler
// viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green
viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue
viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
}
// check whether the translations are relative or absolute
pcl::PointCloud<pcl::PointXYZ>::Ptr best_transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*moved_cloud, *best_transform_cloud, best_fit_transform);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_transform_cloud_color_handler (best_transform_cloud, 245, 20, 20); // red
viewer.addPointCloud (best_transform_cloud, best_transform_cloud_color_handler, "best_transform_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_fit_cloud_color_handler (best_fit_cloud, 20, 245, 20); // green
viewer.addPointCloud (best_transform_cloud, best_fit_cloud_color_handler, "best_fit_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_cloud_color_handler (input_cloud, 255, 255, 255); // white
viewer.addPointCloud (input_cloud, input_cloud_color_handler, "input_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_transform_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_fit_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input_cloud");
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return best_fit_transform;
}
void run()
{
pcl::PointCloud<pcl::PointXYZ> fake_obs;
float z_inc = 1.5;
float X_obs = 1.2;
float Y_obs = -0.4;
pcl::PointXYZ point;
point.x = X_obs;
point.y = Y_obs;
point.z = -0.1;
for(int i=0;i<30; i++)
{
point.x = point.x + costmap_res/30;
point.y = Y_obs;
for(int j=0;j < 30;j++)
{
point.y = point.y - costmap_res/30;
fake_obs.points.push_back(point);
}
point.z = point.z + z_inc/30;
}
X_obs = 1.8;
Y_obs = 1.8;
point.x = X_obs;
point.y = Y_obs;
point.z = -0.1;
for(int i=0;i<30; i++)
{
point.x = point.x + costmap_res/30;
point.y = Y_obs;
for(int j=0;j < 30;j++)
{
point.y = point.y - costmap_res/30;
fake_obs.points.push_back(point);
}
point.z = point.z + z_inc/30;
}
ros::Rate rate(50);
tf::TransformListener listener;
bool first_loop = true;
bool transform_present = true;
float curr_x;
float curr_y;
float curr_yaw;
float last_x;
float last_y;
float last_yaw;
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
while(ros::ok())
{
tf::StampedTransform transform_odom_laser;
try{
listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform_odom_laser);
transform_present = true;
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(0.05).sleep();
transform_present = false;
}
//Reading x and y
curr_x =transform_odom_laser.getOrigin().x();
curr_y =transform_odom_laser.getOrigin().y();
tfScalar roll,pitch,yaw;
tf::Matrix3x3 M(transform_odom_laser.getRotation());
M.getRPY(roll,pitch,yaw,(unsigned int) 1);
curr_yaw = (float) yaw - M_PI/2; //odom orientation is 90 degree rotated with respect to laser
if (!first_loop)
{
float delta_x = (curr_x - last_x) * cos(curr_yaw) + (curr_y - last_y)* sin(curr_yaw);
float delta_y = -(curr_x - last_x) * sin(curr_yaw) + (curr_y - last_y)* cos(curr_yaw);;
float delta_yaw = (curr_yaw - last_yaw);
/*
| cos(yaw) sin(yaw) 0 x|
|-sin(yaw) cos(yaw) 0 y|
| 0 0 1 0|
| 0 0 0 1|
*/
transform_1 (0,0) = cos (delta_yaw); transform_1 (0,1) = sin (delta_yaw);
transform_1 (1,0) = -sin (delta_yaw); transform_1 (1,1) = cos (delta_yaw);
transform_1 (0,3) = -delta_y;
transform_1 (1,3) = -delta_x;
transform_1 (2,3) = 0.0;
pcl::transformPointCloud (fake_obs, fake_obs, transform_1);
}
last_x = curr_x;
last_y = curr_y;
last_yaw = curr_yaw;
if (first_loop && transform_present)
{
first_loop = false;
}
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(fake_obs,cloud);
cloud.header.frame_id = "base_link";
cloud.header.stamp = ros::Time::now();
obstcle_pub_.publish(cloud);
rate.sleep();
ros::spinOnce();
}
}
