void MeshGenerator :: generatePointCloudMesh(const RGBDImage& image,
const Pose3D& depth_pose,
const Pose3D& rgb_pose)
{
m_mesh.clear();
m_mesh.vertices.reserve(image.depth().rows*image.depth().cols);
m_mesh.colors.reserve(image.depth().rows*image.depth().cols);
m_mesh.normals.reserve(image.depth().rows*image.depth().cols);
const cv::Mat1f& depth_im = image.depth();
const cv::Mat1b& mask_im = image.depthMask();
cv::Mat3f voxels (depth_im.size());
cv::Mat3f rgb_points (depth_im.size());
cv::Mat1b subsample_mask(mask_im.size());
subsample_mask = 0;
for (float r = 0; r < subsample_mask.rows-1; r += 1.0/m_resolution_factor)
for (float c = 0; c < subsample_mask.cols-1; c += 1.0/m_resolution_factor)
subsample_mask(ntk::math::rnd(r),ntk::math::rnd(c)) = 1;
subsample_mask = mask_im & subsample_mask;
depth_pose.unprojectFromImage(depth_im, subsample_mask, voxels);
if (m_use_color && image.hasRgb())
rgb_pose.projectToImage(voxels, subsample_mask, rgb_points);
for (int r = 0; r < voxels.rows; ++r)
{
Vec3f* voxels_data = voxels.ptr<Vec3f>(r);
const uchar* mask_data = subsample_mask.ptr<uchar>(r);
for (int c = 0; c < voxels.cols; ++c)
{
if (!mask_data[c])
continue;
Vec3b color (0,0,0);
if (m_use_color)
{
Point3f prgb = rgb_points(r,c);
int i_y = ntk::math::rnd(prgb.y);
int i_x = ntk::math::rnd(prgb.x);
if (is_yx_in_range(image.rgb(), i_y, i_x))
{
Vec3b bgr = image.rgb()(i_y, i_x);
color = Vec3b(bgr[2], bgr[1], bgr[0]);
}
}
else
{
int g = 0;
if (image.intensity().data)
g = image.intensity()(r,c);
else
g = 255 * voxels_data[c][2] / 10.0;
color = Vec3b(g,g,g);
}
m_mesh.vertices.push_back(voxels_data[c]);
m_mesh.colors.push_back(color);
}
}
}
void RGBDFrameRecorder :: writeFrame(const RGBDImage& image, const std::string& frame_dir)
{
std::string raw_frame_dir = format("%s/raw", frame_dir.c_str(), m_frame_index);
QDir dir (frame_dir.c_str());
dir.mkpath("raw");
std::string filename;
if (m_save_rgb_pose && image.calibration())
{
filename = cv::format("%s/rgb_pose.avs", frame_dir.c_str());
image.rgbPose().saveToAvsFile(filename.c_str());
}
if (!m_only_raw)
{
filename = cv::format("%s/color.png", frame_dir.c_str());
imwrite(filename, image.rgb());
}
if (m_save_pcl_point_cloud)
{
filename = cv::format("%s/cloud.pcd", frame_dir.c_str());
#ifdef NESTK_USE_PCL
pcl::PointCloud<pcl::PointXYZ> cloud;
rgbdImageToPointCloud(cloud, image);
pcl::io::savePCDFileASCII(filename.c_str(), cloud);
#endif
}
if (m_use_compressed_format)
filename = cv::format("%s/raw/color.png", frame_dir.c_str());
else
filename = cv::format("%s/raw/color.bmp", frame_dir.c_str());
imwrite(filename, image.rawRgb());
if (!m_only_raw && image.mappedDepth().data)
{
filename = cv::format("%s/mapped_depth.png", frame_dir.c_str());
imwrite_normalized(filename, image.mappedDepth());
filename = cv::format("%s/mapped_color.png", frame_dir.c_str());
imwrite(filename, image.mappedRgb());
filename = cv::format("%s/depth.yml", frame_dir.c_str());
imwrite_yml(filename, image.mappedDepth());
}
if (!m_only_raw)
{
filename = cv::format("%s/raw/depth.png", frame_dir.c_str());
if (image.rawDepth().data)
imwrite_normalized(filename.c_str(), image.rawDepth());
filename = cv::format("%s/depth.png", frame_dir.c_str());
if (image.depth().data)
imwrite_normalized(filename.c_str(), image.depth());
filename = cv::format("%s/intensity.png", frame_dir.c_str());
if (image.intensity().data)
imwrite_normalized(filename.c_str(), image.intensity());
}
if (image.rawDepth().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/depth.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawDepth());
}
else
{
filename = cv::format("%s/raw/depth.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawDepth());
}
}
if (m_save_intensity && image.rawIntensity().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/intensity.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawIntensity());
}
else
{
filename = cv::format("%s/raw/intensity.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawIntensity());
}
}
if (!m_only_raw)
{
filename = cv::format("%s/raw/amplitude.png", frame_dir.c_str());
if (image.rawAmplitude().data)
imwrite_normalized(filename.c_str(), image.rawAmplitude());
filename = cv::format("%s/amplitude.png", frame_dir.c_str());
if (image.amplitude().data)
imwrite_normalized(filename.c_str(), image.amplitude());
}
if (image.rawAmplitude().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/amplitude.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawAmplitude());
}
else
{
filename = cv::format("%s/raw/amplitude.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawAmplitude());
}
}
}
void MeshGenerator :: generateTriangleMesh(const RGBDImage& image,
const Pose3D& depth_pose,
const Pose3D& rgb_pose)
{
const Mat1f& depth_im = image.depth();
const Mat1b& mask_im = image.depthMask();
m_mesh.clear();
if (m_use_color)
image.rgb().copyTo(m_mesh.texture);
else if (image.intensity().data)
toMat3b(normalize_toMat1b(image.intensity())).copyTo(m_mesh.texture);
else
{
m_mesh.texture.create(depth_im.size());
m_mesh.texture = Vec3b(255,255,255);
}
m_mesh.vertices.reserve(depth_im.cols*depth_im.rows);
m_mesh.texcoords.reserve(depth_im.cols*depth_im.rows);
m_mesh.colors.reserve(depth_im.cols*depth_im.rows);
Mat1i vertice_map(depth_im.size());
vertice_map = -1;
for_all_rc(depth_im)
{
if (!mask_im(r,c))
continue;
double depth = depth_im(r,c);
Point3f p3d = depth_pose.unprojectFromImage(Point3f(c,r,depth));
Point3f p2d_rgb;
Point2f texcoords;
if (m_use_color)
{
p2d_rgb = rgb_pose.projectToImage(p3d);
texcoords = Point2f(p2d_rgb.x/image.rgb().cols, p2d_rgb.y/image.rgb().rows);
}
else
{
p2d_rgb = Point3f(c,r,depth);
texcoords = Point2f(p2d_rgb.x/image.intensity().cols, p2d_rgb.y/image.intensity().rows);
}
vertice_map(r,c) = m_mesh.vertices.size();
m_mesh.vertices.push_back(p3d);
// m_mesh.colors.push_back(bgr_to_rgb(im.rgb()(p2d_rgb.y, p2d_rgb.x)));
m_mesh.texcoords.push_back(texcoords);
}
for_all_rc(vertice_map)
{
if (vertice_map(r,c) < 0)
continue;
if ((c < vertice_map.cols - 1) && (r < vertice_map.rows - 1) &&
(vertice_map(r+1,c)>=0) && (vertice_map(r,c+1) >= 0) &&
(std::abs(depth_im(r,c) - depth_im(r+1, c)) < m_max_delta_depth) &&
(std::abs(depth_im(r,c) - depth_im(r, c+1)) < m_max_delta_depth))
{
Face f;
f.indices[2] = vertice_map(r,c);
f.indices[1] = vertice_map(r,c+1);
f.indices[0] = vertice_map(r+1,c);
m_mesh.faces.push_back(f);
}
if ((c > 0) && (r < vertice_map.rows - 1) &&
(vertice_map(r+1,c)>=0) && (vertice_map(r+1,c-1) >= 0) &&
(std::abs(depth_im(r,c) - depth_im(r+1, c)) < m_max_delta_depth) &&
(std::abs(depth_im(r,c) - depth_im(r+1, c-1)) < m_max_delta_depth))
{
Face f;
f.indices[2] = vertice_map(r,c);
f.indices[1] = vertice_map(r+1,c);
f.indices[0] = vertice_map(r+1,c-1);
m_mesh.faces.push_back(f);
}
}
m_mesh.computeNormalsFromFaces();
}
void RGBDFrameRecorder :: saveCurrentFrame(const RGBDImage& image)
{
std::string frame_dir = format("%s/view%04d", m_dir.absolutePath().toStdString().c_str(), m_frame_index);
std::string raw_frame_dir = format("%s/raw", frame_dir.c_str(), m_frame_index);
QDir dir (frame_dir.c_str());
dir.mkpath("raw");
std::string filename;
if (!m_only_raw)
{
filename = cv::format("%s/color.png", frame_dir.c_str());
imwrite(filename, image.rgb());
}
filename = cv::format("%s/raw/color.png", frame_dir.c_str());
imwrite(filename, image.rawRgb());
if (!m_only_raw && image.mappedDepth().data)
{
filename = cv::format("%s/mapped_depth.png", frame_dir.c_str());
imwrite_normalized(filename, image.mappedDepth());
filename = cv::format("%s/mapped_color.png", frame_dir.c_str());
imwrite(filename, image.mappedRgb());
filename = cv::format("%s/depth.yml", frame_dir.c_str());
imwrite_yml(filename, image.mappedDepth());
}
if (!m_only_raw)
{
filename = cv::format("%s/raw/depth.png", frame_dir.c_str());
if (image.rawDepth().data)
imwrite_normalized(filename.c_str(), image.rawDepth());
filename = cv::format("%s/depth.png", frame_dir.c_str());
if (image.depth().data)
imwrite_normalized(filename.c_str(), image.depth());
filename = cv::format("%s/intensity.png", frame_dir.c_str());
if (image.intensity().data)
imwrite_normalized(filename.c_str(), image.intensity());
}
if (image.rawDepth().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/depth.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawDepth());
}
else
{
filename = cv::format("%s/raw/depth.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawDepth());
}
}
if (image.rawIntensity().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/intensity.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawIntensity());
}
else
{
filename = cv::format("%s/raw/intensity.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawIntensity());
}
}
if (!m_only_raw)
{
filename = cv::format("%s/raw/amplitude.png", frame_dir.c_str());
if (image.rawAmplitude().data)
imwrite_normalized(filename.c_str(), image.rawAmplitude());
filename = cv::format("%s/amplitude.png", frame_dir.c_str());
if (image.amplitude().data)
imwrite_normalized(filename.c_str(), image.amplitude());
}
if (image.rawAmplitude().data)
{
if (m_use_binary_raw)
{
filename = cv::format("%s/raw/amplitude.raw", frame_dir.c_str());
imwrite_Mat1f_raw(filename.c_str(), image.rawAmplitude());
}
else
{
filename = cv::format("%s/raw/amplitude.yml", frame_dir.c_str());
imwrite_yml(filename.c_str(), image.rawAmplitude());
}
}
++m_frame_index;
}
