/**
Kreis an die Punkte (2xN-Array) anfitten.
*/
Eigen::Vector3f fit_circle(const Eigen::Array2Xf& points) {
Eigen::Array2f mean = points.rowwise().mean();
Eigen::Array2Xf uv(points.colwise() - mean);
Eigen::Array2f Sp2 = uv.square().rowwise().sum();
Eigen::Array2f Sp3 = uv.cube().rowwise().sum();
float Suv = (uv.row(0) * uv.row(1)).sum();
float Suvv = (uv.row(0) * uv.row(1).square()).sum();
float Svuu = (uv.row(0).square() * uv.row(1)).sum();
// Rechte Seite berechnen
Eigen::Vector2f b((Sp3 + Eigen::Array2f(Suvv, Svuu)) / 2);
// Linke Seite berechnen
Eigen::Matrix2f A;
A << Sp2(0), Suv, Suv, Sp2(1);
// Gleichungssystem Lösen
Eigen::Array2f mid = A.inverse() * b;
// Quadrat des Radius berechnen
float radius = sqrt(mid.square().sum() + Sp2.sum() / points.cols());
return (Eigen::Vector3f() << (mid + mean), radius).finished();
}
void FindObjectOnPlane::generateAngles(
const cv::Mat& blob_image, const cv::Point2f& test_point,
std::vector<double>& angles,
std::vector<double>& max_x,
std::vector<double>& max_y,
const image_geometry::PinholeCameraModel& model,
const jsk_recognition_utils::Plane::Ptr& plane)
{
angles.clear();
const double angle_step = 3;
std::vector<cv::Point> indices;
for (int j = 0; j < blob_image.rows; j++) {
for (int i = 0; i < blob_image.cols; i++) {
if (blob_image.at<uchar>(j, i) != 0) { // need to check
indices.push_back(cv::Point(i, j));
}
}
}
for (double angle = -180; angle < 180; angle += angle_step) {
Eigen::Vector2f ux(cos(angle/180*M_PI), sin(angle/180*M_PI));
//Eigen::Vector2f uy(sin(angle/180*M_PI), -cos(angle/180*M_PI));
cv::Point2d uy_end = getUyEnd(test_point, cv::Point2d(test_point.x + ux[0], test_point.y + ux[1]),
model,
plane);
Eigen::Vector2f uy(uy_end.x - test_point.x, uy_end.y - test_point.y);
Eigen::Matrix2f A;
A << ux[0], uy[0],
ux[1], uy[1];
bool excluded = false;
double max_alpha = -DBL_MAX;
double max_beta = -DBL_MAX;
for (size_t i = 0; i < indices.size(); i++) {
Eigen::Vector2f p_q = Eigen::Vector2f(indices[i].x, indices[i].y) - Eigen::Vector2f(test_point.x, test_point.y);
Eigen::Vector2f a_b = A.inverse() * p_q;
double alpha = a_b[0];
double beta = a_b[1];
if (alpha < 0 || beta < 0) {
excluded = true;
break;
}
if (alpha > max_alpha) {
max_alpha = alpha;
}
if (beta > max_beta) {
max_beta = beta;
}
}
if (!excluded) {
angles.push_back(angle);
max_x.push_back(max_alpha);
max_y.push_back(max_beta);
}
}
}
auto match(vfloat2 p, vfloat2 tr_prediction,
F A, F B, GD Ag,
const int winsize,
const float min_ev_th,
const int max_interations,
const float convergence_delta)
{
typedef typename F::value_type V;
int WS = winsize;
int ws = winsize;
int hws = ws/2;
// Gradient matrix
Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
int cpt = 0;
for(int r = -hws; r <= hws; r++)
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (A.has(n.cast<int>()))
{
Eigen::Matrix2f m;
auto g = Ag.linear_interpolate(n);
float gx = g[0];
float gy = g[1];
m <<
gx * gx, gx * gy,
gx * gy, gy * gy;
G += m;
cpt++;
}
}
// Check minimum eigenvalue.
float min_ev = 99999.f;
auto ev = (G / cpt).eigenvalues();
for (int i = 0; i < ev.size(); i++)
if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());
if (min_ev < min_ev_th)
return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);
Eigen::Matrix2f G1 = G.inverse();
// Precompute gs and as.
vfloat2 prediction_ = p + tr_prediction;
vfloat2 v = prediction_;
Eigen::Vector2f nk = Eigen::Vector2f::Ones();
char gs_buffer[WS * WS * sizeof(vfloat2)];
vfloat2* gs = (vfloat2*) gs_buffer;
// was: vfloat2 gs[WS * WS];
typedef plus_promotion<V> S;
char as_buffer[WS * WS * sizeof(S)];
S* as = (S*) as_buffer;
// was: S as[WS * WS];
{
for(int i = 0, r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
gs[i] = Ag.linear_interpolate(n).template cast<float>();
as[i] = cast<S>(A.linear_interpolate(n));
}
i++;
}
}
}
auto domain = B.domain();// - border(hws + 1);
// Gradient descent
for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
{
Eigen::Vector2f bk = Eigen::Vector2f::Zero();
// Temporal difference.
int i = 0;
for(int r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
vfloat2 n2 = v + vfloat2(r, c);
auto g = gs[i];
float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
}
i++;
}
}
nk = G1 * bk;
v += vfloat2{nk[0], nk[1]};
if (!domain.has(v.cast<int>()))
return std::pair<vfloat2, float>(vfloat2(0, 0), FLT_MAX);
}
// Compute the SSD.
float err = 0;
for(int r = -hws; r <= hws; r++)
for(int c = -hws; c <= hws; c++)
{
vfloat2 n2 = v + vfloat2(r, c);
int i = (r+hws) * ws + (c+hws);
{
err += fabs(cast<float>(as[i] - cast<S>(B.linear_interpolate(n2))));
cpt++;
}
}
return std::pair<vfloat2, float>(v - p, err / (cpt));
}
void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Vector2f mean;
Eigen::Matrix2f S;
if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
S << msg->pose.covariance[0], msg->pose.covariance[1],
msg->pose.covariance[6], msg->pose.covariance[7];
}
else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
S << msg->pose.covariance[7], msg->pose.covariance[8],
msg->pose.covariance[13], msg->pose.covariance[14];
}
else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
S << msg->pose.covariance[14], msg->pose.covariance[12],
msg->pose.covariance[0], msg->pose.covariance[2];
}
double max_sigma = 0;
for (size_t i = 0; i < 2; i++) {
for (size_t j = 0; j < 2; j++) {
double sigma = sqrt(S(i, j));
if (max_sigma < sigma) {
max_sigma = sigma;
}
}
}
Eigen::Matrix2f S_inv = S.inverse();
double dx = 6.0 * max_sigma / sampling_num_;
double dy = 6.0 * max_sigma / sampling_num_;
for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
Eigen::Vector2f diff(x, y);
Eigen::Vector2f input = diff + mean;
float z = gaussian(input, mean, S, S_inv);
pcl::PointXYZ p;
if (cut_plane_ == "xy") {
p.x = input[0];
p.y = input[1];
p.z = z + msg->pose.pose.position.z;
}
else if (cut_plane_ == "yz") {
p.y = input[0];
p.z = input[1];
p.x = z + msg->pose.pose.position.x;
}
else if (cut_plane_ == "zx") {
p.z = input[0];
p.x = input[1];
p.y = z + msg->pose.pose.position.y;
}
else if (cut_plane_ == "flipped_xy") {
p.x = input[0];
p.y = input[1];
p.z = -z + msg->pose.pose.position.z;
}
else if (cut_plane_ == "flipped_yz") {
p.y = input[0];
p.z = input[1];
p.x = -z + msg->pose.pose.position.x;
}
else if (cut_plane_ == "flipped_zx") {
p.z = input[0];
p.x = input[1];
p.y = -z + msg->pose.pose.position.y;
}
cloud->points.push_back(p);
}
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*cloud, ros_cloud);
ros_cloud.header = msg->header;
pub_.publish(ros_cloud);
}
