bool scale_reconstruction::run(viennamesh::algorithm_handle &)
{
point_handle input_points = get_required_input<point_handle>("points");
data_handle<int> sample_option = get_input<int>("neighborhood_sample_size");
data_handle<int> neighborhood_option = get_input<int>("neighborhood_size");
data_handle<int> scale_option = get_input<int>("scale");
Point_collection points;
for (int i = 0; i != input_points.size(); ++i)
points.push_back(Point(input_points(i)[0],
input_points(i)[1],
input_points(i)[2]));
scale_options options;
if(sample_option.valid() && sample_option() > 0)
options.sample_size=sample_option();
else
options.sample_size=100;
if(neighborhood_option.valid() && neighborhood_option() > 0)
options.neighborhood_size=neighborhood_option();
else
options.neighborhood_size=10;
if(scale_option.valid() && scale_option() > 0)
options.scale=scale_option();
else
options.scale=1;
mesh_handle output = make_data<mesh_handle>();
Point_collection & im = points;
scale_reconstruction_impl(im,output(),options);
set_output("mesh", output);
return true;
}
void LSDSLAMToPCL::depthCB(const lsd_slam_to_pcl::keyframeMsgConstPtr msg)
{
if (msg->isKeyframe)
{
pcl::PointCloud<pcl::PointXYZRGB> cloud_pcl;
pcl_ros::PCLNodelet::PointIndices indices_ros;
const float fxi = 1 / msg->fx;
const float fyi = 1 / msg->fy;
const float cxi = -msg->cx / msg->fx;
const float cyi = -msg->cy / msg->fy;
const int width = msg->width;
const int height = msg->height;
int num_pts_total = 0;
boost::shared_ptr<InputPointDense> input_points(new InputPointDense[width * height]);
Sophus::Sim3f cam_to_world = Sophus::Sim3f();
memcpy(cam_to_world.data(), msg->camToWorld.data(), 7 * sizeof(float));
memcpy(input_points.get(), msg->pointcloud.data(), width * height * sizeof(InputPointDense));
const float cam_to_world_scale = cam_to_world.scale();
for (int y = 0; y < height; ++y)
{
for (int x = 0; x < width; ++x)
{
// Catch boundary points
if (x == 0 || y == 0 || x == width - 1 || y == height - 1)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
int point_indice = x + y * width;
const InputPointDense point_in_curr = input_points.get()[point_indice];
if (point_in_curr.idepth <= 0)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
num_pts_total++;
if (sparsify_factor_ > 1 && rand() % sparsify_factor_ != 0)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
const float depth = 1 / point_in_curr.idepth;
const float depth4 = depth * depth * depth * depth;
if (point_in_curr.idepth_var * depth4 > scaled_depth_var_thresh_)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
if (point_in_curr.idepth_var * depth4 * cam_to_world_scale * cam_to_world_scale > abs_depth_var_thresh_)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
if (min_near_support_ > 1)
{
int near_support = 0;
for (int dx = -1; dx < 2; ++dx)
{
for (int dy = -1; dy < 2; ++dy)
{
const InputPointDense point_in_near = input_points.get()[x + dx + (y + dy) * width];
if (point_in_near.idepth > 0)
{
const float diff = point_in_near.idepth - point_in_curr.idepth;
if (diff * diff < 2 * point_in_curr.idepth_var) { near_support++; }
}
}
}
if (near_support < min_near_support_)
{
cloud_pcl.push_back(point_invalid_);
continue;
}
}
Eigen::Vector3f point_eigen = cam_to_world * (Eigen::Vector3f((x * fxi + cxi), (y * fyi + cyi), 1) * depth);
pcl::PointXYZRGB point_pcl;
point_pcl.x = point_eigen[0];
point_pcl.y = point_eigen[1];
point_pcl.z = point_eigen[2];
point_pcl.r = point_in_curr.color[2];
point_pcl.g = point_in_curr.color[1];
point_pcl.b = point_in_curr.color[0];
cloud_pcl.push_back(point_pcl);
indices_ros.indices.push_back(point_indice);
}
}
uint64_t time_stamp = ros::Time::now().toNSec();
cloud_pcl.width = width;
cloud_pcl.height = height;
cloud_pcl.is_dense = false;
indices_ros.header.stamp = pcl_conversions::fromPCL(time_stamp);
cloud_publisher_.publish(boost::make_shared<const pcl::PointCloud<pcl::PointXYZRGB> >(cloud_pcl));
indices_publisher_.publish(boost::make_shared<const pcl_ros::PCLNodelet::PointIndices>(indices_ros));
ROS_DEBUG_STREAM("Published " << num_pts_total << " points to pointcloud, dimensions [" << cloud_pcl.width << " " << cloud_pcl.height << "]");
}
else
{
ROS_DEBUG_STREAM("Error, must subscribe to keyframe");
}
}