test_pointcloud2.cpp
Go to the documentation of this file.
00001 /*
00002  * test_pose_conversions.cpp
00003  *
00004  *  Created on: Mar 15, 2012
00005  *      Author: Pablo IƱigo Blasco
00006  */
00007 
00008 #include <mrpt_bridge/point_cloud2.h>
00009 
00010 #include <mrpt/version.h>
00011 #include <mrpt/maps/CSimplePointsMap.h>
00012 #include <pcl/conversions.h>
00013 #include <pcl/point_cloud.h>
00014 #include <pcl_conversions/pcl_conversions.h>
00015 #include <gtest/gtest.h>
00016 
00017 TEST(PointCloud2, basicTest)
00018 {
00019         pcl::PointCloud<pcl::PointXYZ> point_cloud;
00020 
00021         point_cloud.height = 10;
00022         point_cloud.width = 10;
00023         point_cloud.is_dense = true;
00024 
00025         int num_points = point_cloud.height * point_cloud.width;
00026         point_cloud.points.resize(num_points);
00027 
00028         float i_f = 0;
00029         for (int i = 0; i < num_points; i++)
00030         {
00031                 pcl::PointXYZ& point = point_cloud.points[i];
00032                 point.x = i_f;
00033                 point.y = -i_f;
00034                 point.z = -2 * i_f;
00035                 i_f += 1.0;
00036         }
00037 
00038         sensor_msgs::PointCloud2 point_cloud2_msg;
00039         // pcl_conversions::fromPCL(point_cloud, point_cloud2_msg);
00040         pcl::toROSMsg(point_cloud, point_cloud2_msg);
00041 
00042         mrpt::maps::CSimplePointsMap mrpt_pc;
00043 
00044         // printf("step 3\n");
00045         mrpt_bridge::copy(point_cloud2_msg, mrpt_pc);
00046 
00047         i_f = 0;
00048         for (int i = 0; i < num_points; i++)
00049         {
00050                 float mrpt_x, mrpt_y, mrpt_z;
00051                 mrpt_pc.getPoint(i, mrpt_x, mrpt_y, mrpt_z);
00052                 EXPECT_FLOAT_EQ(mrpt_x, i_f);
00053                 EXPECT_FLOAT_EQ(mrpt_y, -i_f);
00054                 EXPECT_FLOAT_EQ(mrpt_z, -2 * i_f);
00055 
00056                 i_f += 1.0;
00057         }
00058         //;
00059 }


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Apr 27 2018 05:10:54