Eigen::Vector2f StarCamera::undistortRadialTangential(Eigen::Vector2f in) const { float k1 = mDistortionCoeffi(0); float k2 = mDistortionCoeffi(1); float k3 = mDistortionCoeffi(4); float p1 = mDistortionCoeffi(2); float p2 = mDistortionCoeffi(3); float r2; float r4; float kRadial; Eigen::Vector2f Xc = in; // initial guess for(int i=0; i<20; ++i) { r2 = Xc.squaredNorm(); r4 = r2*r2; kRadial = 1 + k1 * r2 + k2 * r4 + k3 * r2*r4; Eigen::Vector2f deltaX; deltaX << 2 * p1 * Xc(0) * Xc(1) + p2 *(r2 + 2 * Xc(0)*Xc(0)), p1 * (r2 + 2 * Xc(1)*Xc(1)) + 2 * p2 * Xc(0) * Xc(1); Xc = (in - deltaX) / kRadial; } return Xc; }
/** Get edge closest to a specified point. * The point must be within an imaginery line segment parallel to * the edge, that is a line perpendicular to the edge must go * through the point and a point on the edge line segment. * @param pos_x X coordinate in global (map) frame of point * @param pos_y X coordinate in global (map) frame of point * @return edge closest to the given point, or invalid edge if * such an edge does not exist. */ NavGraphEdge NavGraph::closest_edge(float pos_x, float pos_y) const { float min_dist = std::numeric_limits<float>::max(); NavGraphEdge rv; Eigen::Vector2f point(pos_x, pos_y); for (const NavGraphEdge &edge : edges_) { const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y()); const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y()); const Eigen::Vector2f direction(target - origin); const Eigen::Vector2f direction_norm = direction.normalized(); const Eigen::Vector2f diff = point - origin; const float t = direction.dot(diff) / direction.squaredNorm(); if (t >= 0.0 && t <= 1.0) { // projection of the point onto the edge is within the line segment float distance = (diff - direction_norm.dot(diff) * direction_norm).norm(); if (distance < min_dist) { min_dist = distance; rv = edge; } } } return rv; }
/** Get the point on edge closest to a given point. * The method determines a line perpendicular to the edge which goes through * the given point, i.e. the point must be within the imaginary line segment. * Then the point on the edge which crosses with that perpendicular line * is returned. * @param x X coordinate of point to get point on edge for * @param y Y coordinate of point to get point on edge for * @return coordinate of point on edge closest to given point * @throw Exception thrown if the point is out of the line segment and * no line perpendicular to the edge going through the given point can * be found. */ cart_coord_2d_t NavGraphEdge::closest_point_on_edge(float x, float y) const { const Eigen::Vector2f point(x, y); const Eigen::Vector2f origin(from_node_.x(), from_node_.y()); const Eigen::Vector2f target(to_node_.x(), to_node_.y()); const Eigen::Vector2f direction(target - origin); const Eigen::Vector2f direction_norm = direction.normalized(); const Eigen::Vector2f diff = point - origin; const float t = direction.dot(diff) / direction.squaredNorm(); if (t >= 0.0 && t <= 1.0) { // projection of the point onto the edge is within the line segment Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm; return cart_coord_2d_t(point_on_line[0], point_on_line[1]); } throw Exception("Point (%f,%f) is not on edge %s--%s", x, y, from_.c_str(), to_.c_str()); }
template <typename PointInT, typename IntensityT> void pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input, const PointCloudInConstPtr& input, const std::vector<FloatImageConstPtr>& prev_pyramid, const std::vector<FloatImageConstPtr>& pyramid, const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints, pcl::PointCloud<pcl::PointUV>::Ptr& keypoints, std::vector<int>& status, Eigen::Affine3f& motion) const { std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ()); Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f); pcl::TransformationFromCorrespondences transformation_computer; const int nb_points = prev_keypoints->size (); for (int level = nb_levels_ - 1; level >= 0; --level) { const FloatImage& prev = *(prev_pyramid[level*3]); const FloatImage& next = *(pyramid[level*3]); const FloatImage& grad_x = *(prev_pyramid[level*3+1]); const FloatImage& grad_y = *(prev_pyramid[level*3+2]); Eigen::ArrayXXf prev_win (track_height_, track_width_); Eigen::ArrayXXf grad_x_win (track_height_, track_width_); Eigen::ArrayXXf grad_y_win (track_height_, track_width_); float ratio (1./(1 << level)); for (int ptidx = 0; ptidx < nb_points; ptidx++) { Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio, prev_keypoints->points[ptidx].v * ratio); Eigen::Array2f next_pt; if (level == nb_levels_ -1) next_pt = prev_pt; else next_pt = next_pts[ptidx]*2.f; next_pts[ptidx] = next_pt; Eigen::Array2i iprev_point; prev_pt -= half_win; iprev_point[0] = floor (prev_pt[0]); iprev_point[1] = floor (prev_pt[1]); if (iprev_point[0] < -track_width_ || (uint32_t) iprev_point[0] >= grad_x.width || iprev_point[1] < -track_height_ || (uint32_t) iprev_point[1] >= grad_y.height) { if (level == 0) status [ptidx] = -1; continue; } float a = prev_pt[0] - iprev_point[0]; float b = prev_pt[1] - iprev_point[1]; Eigen::Array4f weight; weight[0] = (1.f - a)*(1.f - b); weight[1] = a*(1.f - b); weight[2] = (1.f - a)*b; weight[3] = 1 - weight[0] - weight[1] - weight[2]; Eigen::Array3f covar = Eigen::Array3f::Zero (); spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar); float det = covar[0]*covar[2] - covar[1]*covar[1]; float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f; if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ()) { status[ptidx] = -2; continue; } det = 1.f/det; next_pt -= half_win; Eigen::Array2f prev_delta (0, 0); for (unsigned int j = 0; j < max_iterations_; j++) { Eigen::Array2i inext_pt = next_pt.floor ().cast<int> (); if (inext_pt[0] < -track_width_ || (uint32_t) inext_pt[0] >= next.width || inext_pt[1] < -track_height_ || (uint32_t) inext_pt[1] >= next.height) { if (level == 0) status[ptidx] = -1; break; } a = next_pt[0] - inext_pt[0]; b = next_pt[1] - inext_pt[1]; weight[0] = (1.f - a)*(1.f - b); weight[1] = a*(1.f - b); weight[2] = (1.f - a)*b; weight[3] = 1 - weight[0] - weight[1] - weight[2]; // compute mismatch vector Eigen::Array2f beta = Eigen::Array2f::Zero (); mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta); // optical flow resolution Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det); // update position next_pt[0] += delta[0]; next_pt[1] += delta[1]; next_pts[ptidx] = next_pt + half_win; if (delta.squaredNorm () <= epsilon_) break; if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 && std::abs (delta[1] + prev_delta[1]) < 0.01 ) { next_pts[ptidx][0] -= delta[0]*0.5f; next_pts[ptidx][1] -= delta[1]*0.5f; break; } // update delta prev_delta = delta; } // update tracked points if (level == 0 && !status[ptidx]) { Eigen::Array2f next_point = next_pts[ptidx] - half_win; Eigen::Array2i inext_point; inext_point[0] = floor (next_point[0]); inext_point[1] = floor (next_point[1]); if (inext_point[0] < -track_width_ || (uint32_t) inext_point[0] >= next.width || inext_point[1] < -track_height_ || (uint32_t) inext_point[1] >= next.height) { status[ptidx] = -1; continue; } // insert valid keypoint pcl::PointUV n; n.u = next_pts[ptidx][0]; n.v = next_pts[ptidx][1]; keypoints->push_back (n); // add points pair to compute transformation inext_point[0] = floor (next_pts[ptidx][0]); inext_point[1] = floor (next_pts[ptidx][1]); iprev_point[0] = floor (prev_keypoints->points[ptidx].u); iprev_point[1] = floor (prev_keypoints->points[ptidx].v); const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]]; const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]]; transformation_computer.add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0); } } } motion = transformation_computer.getTransformation (); }