Exemplo n.º 1
0
calibu::CameraModelT<Pinhole> CreateScanlineRectifiedLookupAndCameras(
        const Sophus::SE3d T_rl,
        const calibu::CameraModelInterface& cam_left,
        const calibu::CameraModelInterface& cam_right,
        Sophus::SE3d& T_nr_nl,        
        LookupTable& left_lut,
        LookupTable& right_lut
        )
{
    const Sophus::SO3d R_rl = T_rl.so3();
    const Sophus::SO3d R_lr = R_rl.inverse();
    const Eigen::Vector3d l_r = T_rl.translation();
    const Eigen::Vector3d r_l = - (R_lr * l_r);
    
    // Current up vector for each camera (in left FoR)
    const Eigen::Vector3d lup_l = Eigen::Vector3d(0,-1,0);
    const Eigen::Vector3d rup_l = R_lr * Eigen::Vector3d(0,-1,0);
    
    // Hypothetical fwd vector for each camera, perpendicular to baseline (in left FoR)
    const Eigen::Vector3d lfwd_l = (lup_l.cross(r_l)).normalized();
    const Eigen::Vector3d rfwd_l = (rup_l.cross(r_l)).normalized();
    
    // New fwd is average of left / right hypothetical baselines (also perpendicular to baseline)
    const Eigen::Vector3d avgfwd_l = lfwd_l + rfwd_l;
    
    // Define new basis (in left FoR);
    const Eigen::Vector3d x_l = r_l.normalized();
    const Eigen::Vector3d z_l = avgfwd_l.normalized();
    const Eigen::Vector3d y_l = z_l.cross(x_l).normalized();
    
    // New orientation for both left and right cameras (expressed relative to original left)
    Eigen::Matrix3d Rnl_l;
    Rnl_l << x_l, y_l, z_l;
    
    // By definition, the right camera now lies exactly on the x-axis with the same orientation
    // as the left camera.
    T_nr_nl = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-r_l.norm(),0,0) );

    // Work out parameters of new linear camera
    const Range range_width = //Intersection(
                MinMaxRotatedCol(cam_left, Rnl_l);
//                MinMaxRotatedCol(cam_right, Rnl_l)
//                );
    const Range range_height = //Intersection(
                MinMaxRotatedRow(cam_left, Rnl_l);
//                MinMaxRotatedRow(cam_right, Rnl_l)
//                );

    // We want to map range width/height to image via K.    
    const double fu = (cam_left.Width()-1) / range_width.Size();
    const double fv = (cam_left.Height()-1) / range_height.Size();
    const double u0 = -fu * range_width.minr;
    const double v0 = -fv * range_height.minr;
    
    // Setup new camera
    calibu::CameraModelT<Pinhole> new_cam(cam_left.Width(),cam_left.Height());
    new_cam.Params() << fu, fv, u0, v0;    
    
    // Homographies which should be applied to left and right images to scan-line rectify them
    const Eigen::Matrix3d Rl_nlKlinv = Rnl_l.transpose() * new_cam.Kinv();
    const Eigen::Matrix3d Rr_nrKlinv = R_lr.inverse().matrix() * Rnl_l.transpose() * new_cam.Kinv();
    
    CreateLookupTable(cam_left, Rl_nlKlinv, left_lut );
    CreateLookupTable(cam_right, Rr_nrKlinv, right_lut );     
    return new_cam;
}
Exemplo n.º 2
0
// Cluster_DBSCAN::ComputeKdistMap()
void Cluster_DBSCAN::ComputeKdistMap( Range const& Kvals, 
                                      std::vector<int> const& FramesToCluster ) const
{
  int pt1_idx, pt2_idx, d_idx, point;
  mprintf("\tCalculating Kdist map for %s\n", Kvals.RangeArg());
  double* kdist_array; // Store distance of pt1 to every other point.
  int nframes = (int)FramesToCluster.size();
  // Ensure all Kdist points are within proper range
  Range::const_iterator kval;
  for (kval = Kvals.begin(); kval != Kvals.end(); ++kval)
    if (*kval < 1 || *kval >= nframes) {
      mprinterr("Error: Kdist value %i is out of range (1 <= Kdist < %i)\n",
                 *kval, nframes);
      return;
    }
  int nvals = (int)Kvals.Size();
  double** KMAP; // KMAP[i] has the ith nearest point for each point.
  KMAP = new double*[ nvals ];
  for (int i = 0; i != nvals; i++)
    KMAP[i] = new double[ nframes ];
  ParallelProgress progress( nframes );
# ifdef _OPENMP
# pragma omp parallel private(pt1_idx, pt2_idx, d_idx, kval, point, kdist_array) firstprivate(progress)
  {
  progress.SetThread( omp_get_thread_num() );
#endif
  kdist_array = new double[ nframes ];
# ifdef _OPENMP
# pragma omp for
# endif
  for (pt1_idx = 0; pt1_idx < nframes; pt1_idx++) // X
  {
    progress.Update( pt1_idx );
    point = FramesToCluster[pt1_idx];
    d_idx = 0;
    // Store distances from pt1 to pt2
    for (pt2_idx = 0; pt2_idx != nframes; pt2_idx++)
      kdist_array[d_idx++] = FrameDistances_.GetFdist(point, FramesToCluster[pt2_idx]);
    // Sort distances; will be smallest to largest
    std::sort( kdist_array, kdist_array + nframes );
    // Save the distance of specified nearest neighbors to this point.
    d_idx = 0;
    for (kval = Kvals.begin(); kval != Kvals.end(); ++kval) // Y
      KMAP[d_idx++][pt1_idx] = kdist_array[ *kval ];
  }
  delete[] kdist_array;
# ifdef _OPENMP
  } // END omp parallel
# endif
  progress.Finish();
  // Sort all of the individual kdist plots, smallest to largest.
  for (int i = 0; i != nvals; i++)
    std::sort(KMAP[i], KMAP[i] + nframes);
  // Save in matrix, largest to smallest.
  DataSet_MatrixDbl kmatrix;
  kmatrix.Allocate2D( FramesToCluster.size(), Kvals.Size() );
  for (int y = 0; y != nvals; y++) {
    for (int x = nframes - 1; x != -1; x--)
      kmatrix.AddElement( KMAP[y][x] );
    delete[] KMAP[y];
  }
  delete[] KMAP;
  // Write matrix to file
  DataFile outfile;
  ArgList outargs("usemap");
  outfile.SetupDatafile(k_prefix_ + "Kmatrix.gnu", outargs, debug_);
  outfile.AddDataSet( (DataSet*)&kmatrix );
  outfile.WriteDataOut();
  // Write out the largest and smallest values for each K.
  // This means for each value of K the point with the furthest Kth-nearest
  // neighbor etc.
  CpptrajFile maxfile;
  if (maxfile.OpenWrite(k_prefix_ + "Kmatrix.max.dat")) return;
  maxfile.Printf("%-12s %12s %12s\n", "#Kval", "MaxD", "MinD");
  d_idx = 0;
  for (kval = Kvals.begin(); kval != Kvals.end(); ++kval, d_idx++)
    maxfile.Printf("%12i %12g %12g\n", *kval, kmatrix.GetElement(0, d_idx),
                   kmatrix.GetElement(nframes-1, d_idx));
  maxfile.CloseFile();
}