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;
}
// 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();
}
