void OpenSMOKE_ShockTube_Geometry::Setup(const std::string fileName, const std::string name)
{
int i;
profile.AssignFromFile(fileName, name);
// In case of linear profile
if (profile.x.Size() == 2)
{
if (name == "LENGTH")
{
iKind = 1;
D0 = profile.y[1];
slope_diameter = (profile.y[2]-profile.y[1]) / profile.x[2];
}
else if (name == "AREA")
{
iKind = 2;
Area0 = profile.y[1];
slope_area = (profile.y[2]-profile.y[1]) / profile.x[2];
}
}
else
{
if (name == "LENGTH")
{
iKind = 3;
BzzVector z_vector(profile.x.Size());
for(i=1;i<=profile.x.Size()-1;i++)
z_vector[i] = (profile.y[i+1]-profile.y[i]) / (profile.x[i+1]-profile.x[i]);
i = profile.x.Size();
z_vector[i] = (profile.y[i]-profile.y[i-1]) / (profile.x[i]-profile.x[i-1]);
interpolation_diameter(profile.x, profile.y);
interpolation_dDiameter(profile.x, z_vector);
}
else if (name == "AREA")
{
iKind = 4;
BzzVector z_vector(profile.x.Size());
for(i=1;i<=profile.x.Size()-1;i++)
z_vector[i] = (profile.y[i+1]-profile.y[i])/(profile.x[i+1]-profile.x[i]);
i = profile.x.Size();
z_vector[i] = (profile.y[i]-profile.y[i-1])/(profile.x[i]-profile.x[i-1]);
interpolation_area(profile.x, profile.y);
interpolation_dArea(profile.x, z_vector);
}
}
}
void pointCloudModifier::manualTableDetection(pcl::PointCloud<pointT>::Ptr cloud_in,
pcl::PointCloud<pointT>::Ptr cloud_out,
std::vector<pointT> &points){
// Compute the plane coefficients given 3 points
computePlaneCoefficients(points);
// Move the coordenate system of the point cloud to the table plane
// 1st - Set the translation to the first picked point
Eigen::Vector3f point_on_plane_negated(-points[0].x, -points[0].y, -points[0].z);
Eigen::Translation3f translate((Eigen::Vector3f)(point_on_plane_negated));
// 2nd - Find the rotation
Eigen::Vector3f normal_vector(
_planeCoefficients->values[0],
_planeCoefficients->values[1],
_planeCoefficients->values[2]);
Eigen::Vector3f z_vector(0,0,1);
Eigen::Vector3f k_vector = normal_vector.cross(z_vector);
k_vector.normalize();
float angle = acos(normal_vector.dot(z_vector));
Eigen::Affine3f rotate = (Eigen::Affine3f)Eigen::AngleAxisf(angle, k_vector);
// 3rd - Calculate the final transformation
Eigen::Affine3f total_transformation = rotate*translate;
// 4th - Transform the point cloud
pcl::transformPointCloud(
*cloud_in,
*cloud_out,
(Eigen::Affine3f)total_transformation);
}
void pointCloudModifier::automaticTableDetection(pcl::PointCloud<pointT>::Ptr cloud_in,
pcl::PointCloud<pointT>::Ptr cloud_out){
pcl::PointCloud<pointT>::Ptr cloudProjected (new pcl::PointCloud<pointT>);
// Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
// Stores the coefficents to the plane (a * x + b * y + c * z = d)
seg.setInputCloud (cloud_in);
seg.segment (*inliers, *_planeCoefficients);
// Project the model inliers
pcl::ProjectInliers<pointT> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_in);
proj.setModelCoefficients (_planeCoefficients);
proj.filter (*cloudProjected);
// Move the coordinate system of the point cloud to the table plane
// 1st - Set the translation to the center of the plane negated
Eigen::Vector3f cloud_plane_center_negated(0,0,0);
for (size_t i = 0; i < cloudProjected->points.size(); ++i)
{
cloud_plane_center_negated[0] -= cloudProjected->points[i].x/cloudProjected->points.size();
cloud_plane_center_negated[1] -= cloudProjected->points[i].y/cloudProjected->points.size();
cloud_plane_center_negated[2] -= cloudProjected->points[i].z/cloudProjected->points.size();
}
Eigen::Translation3f translation((Eigen::Vector3f)(cloud_plane_center_negated));
// 2nd - Find the rotation
Eigen::Vector3f normal_vector(
_planeCoefficients->values[0],
_planeCoefficients->values[1],
_planeCoefficients->values[2]);
Eigen::Vector3f z_vector(0,0,1);
Eigen::Vector3f k_vector = normal_vector.cross(z_vector);
k_vector.normalize();
float angle = acos(normal_vector.dot(z_vector));
Eigen::Affine3f rotation = (Eigen::Affine3f)Eigen::AngleAxisf(angle, k_vector);
// 3rd - Calculate the final transformation
Eigen::Affine3f total_transformation = rotation*translation;
// 4th - Transform the point cloud
pcl::transformPointCloud(
*cloud_in,
*cloud_out,
(Eigen::Affine3f)total_transformation);
}
void OpenSMOKE_ShockTube_Geometry::Setup(const std::string fileName)
{
int i;
std::string label;
std::string variable;
std::string conversion_x;
std::string conversion_y;
BzzVector x_vector;
BzzVector y_vector;
ifstream fInput;
openInputFileAndControl(fInput, fileName.c_str());
fInput >> label;
if (label != "X")
ErrorMessage("Only the following options are available: X");
fInput >> label;
if (label != "SPACE")
ErrorMessage("Only the following options are available: SPACE");
fInput >> conversion_x;
fInput >> label;
if (label != "Y")
ErrorMessage("Only the following options are available: X");
fInput >> variable;
if (variable != "DIAMETER" && variable != "AREA")
ErrorMessage("Only the following options are available: DIAMETER || AREA");
fInput >> conversion_y;
for(;;)
{
fInput >> label;
if (label == "//") break;
x_vector.Append( atof(label.c_str()) );
fInput >> label;
y_vector.Append( atof(label.c_str()) );
}
// Checking values
if (x_vector[1] != 0.) ErrorMessage("The abscissas must start from 0.0");
if (y_vector[1] <= 0.) ErrorMessage("The initial diameter must be larger than 0.0");
// In case of linear profile
if (x_vector.Size() == 2)
{
if (variable == "DIAMETER")
{
iKind = 1;
x_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_x);
y_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_y);
D0 = y_vector[1];
slope_diameter = (y_vector[2]-y_vector[1]) / x_vector[2];
}
else if (variable == "AREA")
{
iKind = 2;
x_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_x);
y_vector *= OpenSMOKE_Conversions::conversion_area(1.0, conversion_y);
Area0 = y_vector[1];
slope_area = (y_vector[2]-y_vector[1]) / x_vector[2];
}
}
else
{
if (variable == "DIAMETER")
{
iKind = 3;
x_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_x);
y_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_y);
BzzVector z_vector(x_vector.Size());
for(i=1;i<=x_vector.Size()-1;i++)
z_vector[i] = (y_vector[i+1]-y_vector[i]) / (x_vector[i+1]-x_vector[i]);
i = x_vector.Size();
z_vector[i] = (y_vector[i]-y_vector[i-1]) / (x_vector[i]-x_vector[i-1]);
interpolation_diameter(x_vector, y_vector);
interpolation_dDiameter(x_vector, z_vector);
}
else if (variable == "AREA")
{
iKind = 4;
x_vector *= OpenSMOKE_Conversions::conversion_length(1.0, conversion_x);
y_vector *= OpenSMOKE_Conversions::conversion_area(1.0, conversion_y);
BzzVector z_vector(x_vector.Size());
for(i=1;i<=x_vector.Size()-1;i++)
z_vector[i] = (y_vector[i+1]-y_vector[i])/(x_vector[i+1]-x_vector[i]);
i = x_vector.Size();
z_vector[i] = (y_vector[i]-y_vector[i-1])/(x_vector[i]-x_vector[i-1]);
interpolation_area(x_vector, y_vector);
interpolation_dArea(x_vector, z_vector);
}
}
}
