90 output.resize (input_->size ());
91 float nan = std::numeric_limits<float>::quiet_NaN ();
102 end_window_x = std::min (x + window_size_,
static_cast<int> (input_->width)),
103 end_window_y = std::min (y + window_size_,
static_cast<int> (input_->height));
112 static_cast<Eigen::MatrixXf::Index
> (y -
y_w + window_size_));
114 Eigen::VectorXf::Index
d_color =
static_cast<Eigen::VectorXf::Index
> (
115 std::abs ((*input_)[
y_w * input_->width +
x_w].r - (*input_)[y * input_->width + x].r) +
116 std::abs ((*input_)[
y_w * input_->width +
x_w].g - (*input_)[y * input_->width + x].g) +
117 std::abs ((*input_)[
y_w * input_->width +
x_w].b - (*input_)[y * input_->width + x].b));
121 if (std::isfinite ((*input_)[
y_w*input_->width +
x_w].z))
128 output[y*input_->width + x].r = (*input_)[y*input_->width + x].r;
129 output[y*input_->width + x].g = (*input_)[y*input_->width + x].g;
130 output[y*input_->width + x].b = (*input_)[y*input_->width + x].b;
135 Eigen::Vector3f
pc (
static_cast<float> (x) * depth,
static_cast<float> (y) * depth, depth);
136 Eigen::Vector3f
pw (unprojection_matrix_ *
pc);
137 output[y*input_->width + x].x =
pw[0];
138 output[y*input_->width + x].y =
pw[1];
139 output[y*input_->width + x].z =
pw[2];
149 output.header = input_->header;
150 output.width = input_->width;
151 output.height = input_->height;