Ejemplo n.º 1
0
  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;
  }
Ejemplo n.º 2
0
  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());
        }
      }
  }
Ejemplo n.º 3
0
void calibrate_kinect_depth(std::vector< DepthCalibrationPoint >& depth_values)
{
    std::vector< std::vector<Point3f> > pattern_points;
    calibrationPattern(pattern_points,
                       global::opt_pattern_width(),  global::opt_pattern_height(), global::opt_square_size(),
                       global::images_list.size());

    for (int i_image = 0; i_image < global::images_list.size(); ++i_image)
    {
        // Generate depth calibration points
        QString filename = global::images_list[i_image];
        QDir cur_image_dir (global::images_dir.absoluteFilePath(filename));
        std::string full_filename = cur_image_dir.absoluteFilePath("raw/color.png").toStdString();
        RGBDImage image;
        OpenniRGBDProcessor processor;
        processor.setFilterFlag(RGBDProcessorFlags::ComputeMapping, true);
        image.loadFromDir(cur_image_dir.absolutePath().toStdString(), &global::calibration, &processor);

        imshow_normalized("mapped depth", image.mappedDepth());
        imshow("color", image.rgb());

        std::vector<Point2f> current_view_corners;
        calibrationCorners(full_filename, "corners",
                           global::opt_pattern_width(), global::opt_pattern_height(),
                           current_view_corners, image.rgb(), 1, global::pattern_type);

        if (current_view_corners.size() != (global::opt_pattern_width()*global::opt_pattern_height()))
        {
            ntk_dbg(1) << "Corners not detected in " << cur_image_dir.absolutePath().toStdString();
            continue;
        }

        // FIXME: why rvecs and tvecs from calibrateCamera seems to be nonsense ?
        // calling findExtrinsics another time to get good chessboard estimations.

        cv::Mat1f H;
        estimate_checkerboard_pose(pattern_points[0],
                                   current_view_corners,
                                   global::calibration.rgb_intrinsics,
                                   H);
        Pose3D pose;
        pose.setCameraParametersFromOpencv(global::calibration.rgb_intrinsics);
        pose.setCameraTransform(H);
        ntk_dbg_print(pose, 1);

        cv::Mat3b debug_img;
        image.rgb().copyTo(debug_img);
        foreach_idx(pattern_i, pattern_points[0])
        {
            Point3f p = pose.projectToImage(pattern_points[0][pattern_i]);
            ntk_dbg_print(p, 1);
            float kinect_raw = image.mappedDepth()(p.y, p.x);
            ntk_dbg_print(kinect_raw, 1);
            if (kinect_raw < 1e-5) continue;
            float err = kinect_raw-p.z;
            cv::putText(debug_img, format("%d", (int)(err*1000)), Point(p.x, p.y), CV_FONT_NORMAL, 0.4, Scalar(255,0,0));
            ntk_dbg_print(pattern_points[0][pattern_i], 1);
            ntk_dbg_print(p, 1);
            ntk_dbg_print(kinect_raw, 1);
            depth_values.push_back(DepthCalibrationPoint(kinect_raw, p.z));
        }
        imshow("errors", debug_img);
        cv::waitKey(0);
    }