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);
}
}
}
// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
static void ComputeOBB(b2OBB* obb, const b2Vec2* vs, int32 count)
{
b2Assert(count <= b2_maxPolygonVertices);
b2Vec2 p[b2_maxPolygonVertices + 1];
for (int32 i = 0; i < count; ++i)
{
p[i] = vs[i];
}
p[count] = p[0];
float32 minArea = B2_FLT_MAX;
for (int32 i = 1; i <= count; ++i)
{
b2Vec2 root = p[i-1];
b2Vec2 ux = p[i] - root;
//@TODO: put back in for assertions, in release
#ifdef _DEBUG
float32 length = ux.Normalize();
b2Assert(length > B2_FLT_EPSILON);
#endif
b2Vec2 uy(-ux.y, ux.x);
b2Vec2 lower(B2_FLT_MAX, B2_FLT_MAX);
b2Vec2 upper(-B2_FLT_MAX, -B2_FLT_MAX);
for (int32 j = 0; j < count; ++j)
{
b2Vec2 d = p[j] - root;
b2Vec2 r;
r.x = b2Dot(ux, d);
r.y = b2Dot(uy, d);
lower = b2Min(lower, r);
upper = b2Max(upper, r);
}
float32 area = (upper.x - lower.x) * (upper.y - lower.y);
if (area < 0.95f * minArea)
{
minArea = area;
obb->R.col1 = ux;
obb->R.col2 = uy;
b2Vec2 center = 0.5f * (lower + upper);
obb->center = root + b2Mul(obb->R, center);
obb->extents = 0.5f * (upper - lower);
}
}
b2Assert(minArea < B2_FLT_MAX);
}
void load_fields_on_fft( const Triangulation& T , CH_FFT& fft ) {
int Nb = fft.Nx();
size_t align=fft.alignment();
c_array ux( Nb , Nb , align );
c_array uy( Nb , Nb , align );
for(F_v_it vit=T.vertices_begin();
vit != T.vertices_end();
vit++) {
int nx = vit->nx.val();
int ny = vit->ny.val();
// "right" ordering
int i = ( Nb - 1 ) - ny ;
int j = nx;
// "wrong" ordering
// int i = nx;
// int j = ny;
Vector_2 vv = vit->Uold.val();
//FT val = vit->alpha.val();
ux(i,j) = vv.x();
uy(i,j) = vv.y();
}
fft.set_u( ux , uy );
return;
}
void PenWindow::layout()
{
CEGUI::UDim ux(0,10);
CEGUI::UDim uy(0,0);
CEGUI::UDim uw(0,0);
CEGUI::UDim uh(0,0);
CEGUI::Size size = getPixelSize();
{
CEGUI::UVector2 bsize;
bsize.d_x = CEGUI::UDim(0,BUTTON_SIZE);
bsize.d_y = CEGUI::UDim(0,BUTTON_SIZE);
const int cellsize = (BUTTON_SIZE + SPACE_SIZE);
int nx = (int)(size.d_width - (2*MARGIN_SIZE)) / cellsize;
if (nx < 1) { nx = 1; }
int xi = 0;
int yi = 0;
CEGUI::UDim x(0,0),y(0,0);
for (size_t i=0; i<_button.size(); ++i) {
for (size_t j=0; j<_button[i].size(); ++j) {
x.d_offset = MARGIN_SIZE + (0.5f * SPACE_SIZE) + xi*cellsize;
y.d_offset = MARGIN_SIZE + (0.5f * SPACE_SIZE) + yi*cellsize;
_button[i][j]->setSize(bsize);
_button[i][j]->setPosition(CEGUI::UVector2(x,y));
xi++;
if (xi == nx) {
xi = 0;
yi++;
}
}
yi++;
xi = 0;
}
}
}
double FindObjectOnPlane::drawAngle(
cv::Mat& out_image, const cv::Point2f& test_point, const double angle,
const double max_x, const double max_y,
const image_geometry::PinholeCameraModel& model,
const jsk_recognition_utils::Plane::Ptr& plane,
cv::Scalar color)
{
Eigen::Vector2f ux(cos(angle/180*M_PI), sin(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::Vector2f uy(sin(angle/180*M_PI), -cos(angle/180*M_PI));
Eigen::Vector2f to_point = ux * max_x + Eigen::Vector2f(test_point.x, test_point.y);
Eigen::Vector2f to_point2 = uy * max_y + Eigen::Vector2f(test_point.x, test_point.y);
cv::Point2f to_point_cv(to_point[0], to_point[1]);
cv::Point2f to_point2_cv(to_point2[0], to_point2[1]);
cv::Point2f to_point3_cv = to_point_cv + to_point2_cv - test_point;
cv::line(out_image, test_point, to_point_cv, color, 4);
cv::line(out_image, test_point, to_point2_cv, color, 4);
cv::line(out_image, to_point_cv, to_point3_cv, color, 4);
cv::line(out_image, to_point2_cv, to_point3_cv, color, 4);
return max_x * max_y; // TODO: it's not correct!
}
/// Get up unit vector
Vec3d uu() const { return uy(); }
void MMS(vector<myData> & F_out, vector<myData> & Ftt_out, FDM::baseSolver& sol, const vector<myData>& u,int step,double t, void * args){
double x = 0;
double y = 0;
double z = 0;
int LN_X = sol.X_LayerNumber();
int LN_Y = sol.Y_LayerNumber();
int LN_Z = sol.Z_LayerNumber();
int Nx,Ny,Nz;
int index = 0;
for (int lz = 0; lz < LN_Z; lz++) {
for (int ly = 0; ly < LN_Y; ly++) {
for (int lx = 0; lx < LN_X; lx++) {
index = idx(lx, ly, lz);
Nx = u[index].Nx();
Ny = u[index].Ny();
Nz = u[index].Nz();
myData ux(Nx,Ny,Nz);
myData uy(Nx,Ny,Nz);
myData uz(Nx,Ny,Nz);
myData Bux(Nx,Ny,Nz);
myData Buy(Nx,Ny,Nz);
myData Buz(Nx,Ny,Nz);
for (int k = 0; k < Nz; k++) {
z = sol.Z(k,lz);
for (int j = 0; j < Ny; j++) {
y = sol.Y(j,ly);
for (int i = 0; i < Nx; i++) {
x = sol.X(i,lx);
F_out[index](i,j,k) = 8*M_PI*M_PI*cos(2*M_PI*t)*cos(2*M_PI*x)*cos(2*M_PI*y)*cos(2*M_PI*z);
Ftt_out[index](i,j,k) = -4*M_PI*M_PI*F_out[index](i,j,k);
ux(i,j,k) = -2*M_PI*cos(2*M_PI*t)*sin(2*M_PI*x)*cos(2*M_PI*y)*cos(2*M_PI*z);//ux boundary
uy(i,j,k) = -2*M_PI*cos(2*M_PI*t)*cos(2*M_PI*x)*sin(2*M_PI*y)*cos(2*M_PI*z);//uy
uz(i,j,k) = -2*M_PI*cos(2*M_PI*t)*cos(2*M_PI*x)*cos(2*M_PI*y)*sin(2*M_PI*z);//uz
}
}
}
//Bux setup
if (lx == 0) {
for (int k = 0; k < Nz; k++) {
for (int j = 0; j < Ny; j++) {
Bux(0,j,k) = -1;
}
}
}
if (lx == LN_X - 1) {
for (int k = 0; k < Nz; k++) {
for (int j = 0; j < Ny; j++) {
Bux(Nx - 1,j,k) = 1;
}
}
}
//
///Buy setup
if (ly == 0) {
for (int k = 0; k < Nz; k++) {
for (int i = 0; i < Nx; i++) {
Buy(i,0,k) = -1;
}
}
}
if (ly == LN_Y - 1) {
for (int k = 0; k < Nz; k++) {
for (int i = 0; i < Nx; i++) {
Buy(i,Ny - 1,k) = 1;
}
}
}
//
//Buz setup
if (lz == 0) {
for (int j = 0; j < Ny; j++) {
for (int i = 0; i < Nx; i++) {
Buz(i,j,0) = -1;
}
}
}
if (lz == LN_Z - 1) {
for (int j = 0; j < Ny; j++) {
for (int i = 0; i < Nx; i++) {
Buz(i,j,Nz - 1) = 1;
}
}
}
//
F_out[index] += sol.H_INV()*(Bux*ux + Buy*uy + Buz*uz);
}
}
}
}
