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