C++ Vector3f::normalized方法代码示例

inline double
pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
{
    // Compute the actual angle
    double rad = v1.normalized ().dot (v2.normalized ());
    if (rad < -1.0)
        rad = -1.0;
    else if (rad >  1.0)
        rad = 1.0;
    return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
}

void
pd_solver_advance(struct PdSolver *solver, uint32_t const n_iterations)
{
        /* set external force */
        Eigen::VectorXf ext_accel = Eigen::VectorXf::Zero(solver->positions.size());
#pragma omp parallel for
        for (uint32_t i = 0; i < solver->positions.size()/3; ++i)
                for (int j = 0; j < 3; ++j)
                        ext_accel[3*i + j] = solver->ext_force[j];
        Eigen::VectorXf const ext_force = solver->mass_mat*ext_accel;
        Eigen::VectorXf const mass_y = solver->mass_mat*(2.0f*solver->positions - solver->positions_last);

        solver->positions_last = solver->positions;

        for (uint32_t iter = 0; iter < n_iterations; ++iter) {
                /* LOCAL STEP */
                /* LOCAL STEP (we account everything except the global solve */
                struct timespec local_start;
                clock_gettime(CLOCK_MONOTONIC, &local_start);
                uint32_t const n_constraints = solver->n_attachments + solver->n_springs;
                Eigen::VectorXf d(3*n_constraints);
                for (uint32_t i = 0; i < solver->n_attachments; ++i) {
                        struct PdConstraintAttachment c = solver->attachments[i];
                        d.block<3, 1>(3*i, 0) = Eigen::Map<Eigen::Vector3f>(c.position);
                }

                uint32_t const offset = solver->n_attachments;
                for (uint32_t i = 0; i < solver->n_springs; ++i) {
                        struct PdConstraintSpring c = solver->springs[i];
                        Eigen::Vector3f const v = solver->positions.block<3, 1>(3*c.i[1], 0)
                                - solver->positions.block<3, 1>(3*c.i[0], 0);
                        d.block<3, 1>(3*(offset + i), 0) = v.normalized()*c.rest_length;
                }

                Eigen::VectorXf const b = mass_y + solver->t2*(solver->j_mat*d + ext_force);
                struct timespec local_end;
                clock_gettime(CLOCK_MONOTONIC, &local_end);

                /* GLOBAL STEP */
                struct timespec global_start;
                clock_gettime(CLOCK_MONOTONIC, &global_start);
                solve(solver, b);
                struct timespec global_end;
                clock_gettime(CLOCK_MONOTONIC, &global_end);

                solver->global_time = pd_time_diff_ms(&global_start, &global_end);
                solver->local_time = pd_time_diff_ms(&local_start, &local_end);

                solver->global_cma = (solver->global_time + solver->n_iters*solver->global_cma)/(solver->n_iters + 1);
                solver->local_cma = (solver->local_time + solver->n_iters*solver->local_cma)/(solver->n_iters + 1);

                if (!(solver->n_iters % 1000)) {
                        printf("Local CMA: %f ms\n", solver->local_cma);
                        printf("Global CMA: %f ms\n\n", solver->global_cma);
                }

                ++solver->n_iters;
        }
}

void stateCallback(const nav_msgs::Odometry::ConstPtr& state)
{
  
	
		vel[0] = state->twist.twist.linear.x;
		vel[1] = state->twist.twist.linear.y;
		vel[2] = state->twist.twist.linear.z;
		pos[2] = state->pose.pose.position.z;
    
		//q.x() = state->pose.pose.orientation.x;
		//q.y() = state->pose.pose.orientation.y;
		//q.z() = state->pose.pose.orientation.z;
		//q.w() = state->pose.pose.orientation.w; 
    float q_x = state->pose.pose.orientation.x;
		float q_y = state->pose.pose.orientation.y;
		float q_z = state->pose.pose.orientation.z;
		float q_w = state->pose.pose.orientation.w;

float  yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z));
//Eigen::Matrix3d R(q);
	force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0);
	if(pd_control==0)
  force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1);
  else if(pd_control==1)
  {
    pos[1]=state->pose.pose.position.y;
    force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos[1])+m*acc_des(1);
  }
	force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2));


  b3 = force.normalized();
  b2 = b3.cross(b1w);
  b1 = b2.cross(b3);
  R_des<<b1[0],b2[0],b3[0],
    b1[1],b2[1],b3[1],
    b1[2],b2[2],b3[2];
  Eigen::Quaternionf q_des(R_des);
  	
  quadrotor_msgs::SO3Command command;
	command.force.x = force[0];
  command.force.y = force[1];
  command.force.z = force[2];
  command.orientation.x = q_des.x();
  command.orientation.y = q_des.y();
  command.orientation.z = q_des.z();
  command.orientation.w = q_des.w();
  command.kR[0] = k_R;
  command.kR[1] = k_R;
  command.kR[2] = k_R;
  command.kOm[0] = k_omg;
  command.kOm[1] = k_omg;
  command.kOm[2] = k_omg;
	command.aux.current_yaw = yaw;
	command.aux.enable_motors = true;
	command.aux.use_external_yaw = true;
  control_pub.publish(command);
}

 Point ModelView::make_exocentric_focus__(
     Eigen::Vector3f const & view_direction,
     Point const & view_origin)
 {
     Eigen::Vector3f unit_viewdir = view_direction.normalized();
     return Point(view_origin.x() + unit_viewdir.x(),
                  view_origin.y() + unit_viewdir.y(),
                  view_origin.z() + unit_viewdir.z());
 }

	Eigen::Matrix4f MatrixFactory::createLookAt
	(
		const Eigen::Vector3f& position,
		const Eigen::Vector3f& direction,
		const Eigen::Vector3f& world_up,
		Handedness handedness
	) const
	{
		if (handedness == Handedness::RightHanded)
		{
			Eigen::Vector3f right, up, dir;
			dir = -direction.normalized();
			right = world_up.normalized().cross(dir).normalized();
			up = dir.cross(right);

			Eigen::Matrix4f matrix;
			matrix << right.x(), right.y(), right.z(), -right.dot(position),
				      up.x(),    up.y(),    up.z(),    -up.dot(position),
				      dir.x(),   dir.y(),   dir.z(),   -dir.dot(position),
				      0.0f, 0.0f, 0.0f, 1.0f;

			return matrix;
		}
		else if (handedness == Handedness::LeftHanded)
		{
			Eigen::Vector3f right, up, dir;
			dir = direction.normalized();
			right = world_up.normalized().cross(dir).normalized();
			up = dir.cross(right);

			Eigen::Matrix4f matrix;
			matrix << right.x(), right.y(), right.z(), -right.dot(position),
				      up.x(),    up.y(),    up.z(),    -up.dot(position),
				      dir.x(),   dir.y(),   dir.z(),   -dir.dot(position),
				      0.0f, 0.0f, 0.0f, 1.0f;

			return matrix;
		}
		else
		{
			DebugError("Not implemented.");
			return Eigen::Matrix4f::Zero();
		}
	}

void computeTransformationFromYZVectorsAndOrigin(const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
  const Eigen::Vector3f& origin, Eigen::Affine3f& transformation){

 Eigen::Vector3f x = (y_direction.cross(z_axis)).normalized();
 Eigen::Vector3f y = y_direction.normalized();
 Eigen::Vector3f z = z_axis.normalized();

 Eigen::Affine3f sub = Eigen::Affine3f::Identity();
 sub(0,3) = -origin[0];
 sub(1,3) = -origin[1];
 sub(2,3) = -origin[2];


 transformation = Eigen::Affine3f::Identity();
 transformation(0,0)=x[0]; transformation(0,1)=x[1]; transformation(0,2)=x[2]; // x^t
 transformation(1,0)=y[0]; transformation(1,1)=y[1]; transformation(1,2)=y[2]; // y^t
 transformation(2,0)=z[0]; transformation(2,1)=z[1]; transformation(2,2)=z[2]; // z^t

 transformation = transformation*sub;
}

  void
  Cylinder::updateAttributes(const Eigen::Vector3f& sym_axis, const Eigen::Vector3f& origin, const Eigen::Vector3f& z_axis)
  {
    //origin_= new_origin;
    sym_axis_ = sym_axis;
    if (sym_axis_[2] < 0 )
    {
      sym_axis_ = sym_axis_ * -1;
    }
    computePose(origin, z_axis.normalized());
    //d_ = fabs(pose_.translation().dot(normal_));
    //normal_ = new_normal;

    /*Eigen::Affine3f transform_from_plane_to_world;
  getTransformationFromPlaneToWorld(sym_axis,normal,origin_,transform_from_plane_to_world);
  transform_from_world_to_plane=transform_from_plane_to_world.inverse();*/
  }

bool Camera::collides(const Entity &e) {
//    return false;
    float cam_rad = collisionRadius();

    if (e.useBoundingBox()) {
        //      return true;
        BoundingBox bb = e.getBoundingBox();
        bb.box.min += e.getPosition();
        bb.box.max += e.getPosition();
        Eigen::Vector3f our_pos = -translations;
        if (bb.contains(our_pos)) {
            return true;
        }

        Eigen::Vector3f displacement = (bb.box.center() - our_pos);
        float distance = displacement.norm();
        Eigen::Vector3f direction = displacement.normalized();

        if (bb.contains(direction * cam_rad + our_pos)) {
            return true;
        }

        return false;
        
    }
    else {
        Eigen::Vector3f their_pos = e.getCenterWorld();
        Eigen::Vector3f our_pos = translations;
        their_pos(1) = 0;
        our_pos(1) = 0;
        Eigen::Vector3f delta = -their_pos - our_pos;
        return std::abs(delta.norm()) < cam_rad + e.getRadius();
    }
    
    
    /*
    std::cout << "object pos" << std::endl;
    std::cout << their_pos << std::endl;
    std::cout << "cam pos" << std::endl;
    std::cout << our_pos << std::endl;
    std::cout << " dist = " << std::abs(delta.norm()) << std::endl;
    */

    
}

  void
  Cylinder::getMergeCandidates(const std::vector<Polygon::Ptr>& cylinder_array,
                               std::vector<int>& intersections)
  {

    for (size_t i = 0; i < cylinder_array.size(); i++)
    {
      Cylinder::Ptr c(boost::dynamic_pointer_cast<Cylinder>(cylinder_array[i]));
      BOOST_ASSERT(c);
      Cylinder& c_map = *c;
      Eigen::Vector3f connection = c_map.pose_.translation() - pose_.translation();
      //connection.normalize();
      //Eigen::Vector3f d= c_map.origin_  - this->origin_   ;

      // Test for geometrical attributes of cylinders
      if(fabs(c_map.sym_axis_.dot(sym_axis_)) > merge_settings_.angle_thresh && (fabs(c_map.r_ - r_) < (0.1 ))) //TODO: add param
      {
        // Test for spatial attributes of cylinders
        if( connection.norm() < (c_map.r_ + 0.1) || fabs(c_map.sym_axis_.dot(connection.normalized())) > merge_settings_.angle_thresh )
        {
          /*Cylinder::Ptr c1(new Cylinder);
        Cylinder::Ptr c2(new Cylinder);
           *c1 = *this;
           *c2 = c_map;
        c2->pose_ = c1->pose_;
        //c2->transform_from_world_to_plane = c1->transform_from_world_to_plane;
        c1->makeCyl2D();
        c2->makeCyl2D();*/
          if(isIntersectedWith(cylinder_array[i]))
            //if (c1->isIntersectedWith(c2))
          {
            intersections.push_back(i);
          }
        }
      }
    }
  }

 Plane::Plane(Eigen::Vector3f normal, double d) :
   normal_(normal.normalized()), d_(d / normal.norm())
 {
   initializeCoordinates();
 }

template<typename PointT> void
pcl::BoxClipper3D<PointT>::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size)
{
  transformation_ = Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (box_size);
  //inverse_transformation_ = transformation_.inverse ();
}

//.........这里部分代码省略.........
		else if(t<des_t-t3&&t>=t1){
			vel_des[0]=des_vx;
			vel_des[1]=des_vy;
			acc_des = Eigen::Vector3f::Zero();
		}
		else if(t>=des_t-t3&&t<des_t){
			vel_des[0] = 0;
			vel_des[1] = 0;
      acc_des[0] = -ax3;
      acc_des[1] = -ay3;
      acc_des[2] = 0;
      stop = 1;
		}
		else if(t>=des_t){
      stop = 0;
			vel_des = Eigen::Vector3f::Zero();
			acc_des = Eigen::Vector3f::Zero();
		}
		vel_des[0] = vel_des[0]+rc_vx/2;
		vel_des[1] = vel_des[1]+rc_vy/2;
	}

	vel(0) = state->twist.twist.linear.x;
	vel(1) = state->twist.twist.linear.y;
	vel(2) = state->twist.twist.linear.z;
	pos(2) = state->pose.pose.position.z;

	//q.x() = state->pose.pose.orientation.x;
	//q.y() = state->pose.pose.orientation.y;
	//q.z() = state->pose.pose.orientation.z;
	//q.w() = state->pose.pose.orientation.w; 
	float q_x = state->pose.pose.orientation.x;
	float q_y = state->pose.pose.orientation.y;
	float q_z = state->pose.pose.orientation.z;
	float q_w = state->pose.pose.orientation.w;

	float  yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z));
	if(stop==0){
		force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0);
		if(pd_control==0)
			force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1);
		else if(pd_control==1)
		{
			pos(1)=state->pose.pose.position.y;
			force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos(1))+m*acc_des(1);
		}
		force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2));
	}
	else if(stop==1)
	{
		force(0) = offset_x+m*acc_des(0);
		force(1) = offset_y+m*acc_des(1);
  }


	b3 = force.normalized();
	b2 = b3.cross(b1w);
	b1 = b2.cross(b3);
	R_des<<b1[0],b2[0],b3[0],
    b1[1],b2[1],b3[1],
    b1[2],b2[2],b3[2];
  Eigen::Quaternionf q_des(R_des);
  	
  
  quadrotor_msgs::SO3Command command;
	command.force.x = force[0];
  command.force.y = force[1];
  command.force.z = force[2];
  command.orientation.x = q_des.x();
  command.orientation.y = q_des.y();
  command.orientation.z = q_des.z();
  command.orientation.w = q_des.w();
  command.kR[0] = k_R;
  command.kR[1] = k_R;
  command.kR[2] = k_R;
  command.kOm[0] = k_omg;
  command.kOm[1] = k_omg;
  command.kOm[2] = k_omg;
	command.aux.current_yaw = yaw;
	command.aux.enable_motors = true;
	command.aux.use_external_yaw = true;
  control_pub.publish(command);

opticalflow_msgs::Traj traj;

	traj.x = pos_des[0];
	traj.y = pos_des[1];
	traj.z = pos_des[2];
	traj.vx = vel_des[0];
	traj.vy = vel_des[1];
	traj.vz = vel_des[2];
	traj.acc_x = acc_des[0];
	traj.acc_y = acc_des[1];
	traj.acc_z = acc_des[2];
	traj.stop = 0;
	traj.mode = mode;
	traj.use_sensor = true;
	traj_pub.publish(traj);

}

std::vector<Vector3f> ShapeEstimation::gradientField(ImageReader mask, std::vector<float> &pixelLuminances, std::vector<float> &gradientX, std::vector<float> &gradientY){
    int rows = mask.getImageHeight();
    int cols = mask.getImageWidth();

    float gxNormalize = 0.0f;
    float gyNormalize = 0.0f;

    for(int row = 0; row < rows; row++){
        for(int col = 0; col < cols; col++){
           int index = mask.indexAt(row, col);
           if(row + 1 < rows){
               int indexUp = mask.indexAt(row + 1, col);
               float dY = pixelLuminances[indexUp] - pixelLuminances[index];
               gradientY.push_back(dY);
               if(fabs(dY) > gyNormalize){
                   gyNormalize = fabs(dY);
               }

           } else {
               gradientY.push_back(0.0f);
           }

           if(col + 1 < cols){
               int indexRight = mask.indexAt(row, col+1);
               float dX = pixelLuminances[indexRight] - pixelLuminances[index];
               gradientX.push_back(dX);
               if(fabs(dX) > gxNormalize){
                   gxNormalize = fabs(dX);
               }
           } else {
               gradientX.push_back(0.0f);
           }

        }
    }
    assert(gradientX.size() == gradientY.size());

    for(int i = 0; i < gradientX.size(); i++){
        gradientX[i] = gradientReshapeRecursive(gradientX[i]/gxNormalize, m_curvature) * gxNormalize;

    }
    for(int i = 0; i < gradientY.size(); i++){
        gradientY[i] = gradientReshapeRecursive(gradientY[i]/gyNormalize, m_curvature) * gyNormalize;
    }

    std::vector<Vector3f> normals;
    for(int i = 0; i < rows; i++){
        for(int j = 0; j < cols; j++){
            QColor maskColor = QColor(mask.pixelAt(i,j));
            if(maskColor.red() > 150){
                Eigen::Vector3f gx = Vector3f(1.0f, 0.0f, gradientX[mask.indexAt(i,j)]);
                Eigen::Vector3f gy = Vector3f(0.0f, 1.0f, gradientY[mask.indexAt(i,j)]);


                Eigen::Vector3f normal = (gx.cross(gy));

                normal = normal.normalized();
                normals.push_back(normal);
            } else {
                normals.push_back(Vector3f(0.0,0.0,0.0));
            }
        }
    }
    //pixelLuminances = gradientX;
    return normals;
}

//.........这里部分代码省略.........
                break;
                case EFloat: {
                    check_attributes(node, { "name", "value" });
                    list.setFloat(node.attribute("name").value(), toFloat(node.attribute("value").value()));
                }
                break;
                case EInteger: {
                    check_attributes(node, { "name", "value" });
                    list.setInteger(node.attribute("name").value(), toInt(node.attribute("value").value()));
                }
                break;
                case EBoolean: {
                    check_attributes(node, { "name", "value" });
                    list.setBoolean(node.attribute("name").value(), toBool(node.attribute("value").value()));
                }
                break;
                case EPoint: {
                    check_attributes(node, { "name", "value" });
                    list.setPoint(node.attribute("name").value(), Point3f(toVector3f(node.attribute("value").value())));
                }
                break;
                case EVector: {
                    check_attributes(node, { "name", "value" });
                    list.setVector(node.attribute("name").value(), Vector3f(toVector3f(node.attribute("value").value())));
                }
                break;
                case EColor: {
                    check_attributes(node, { "name", "value" });
                    list.setColor(node.attribute("name").value(), Color3f(toVector3f(node.attribute("value").value()).array()));
                }
                break;
                case ETransform: {
                    check_attributes(node, { "name" });
                    list.setTransform(node.attribute("name").value(), transform.matrix());
                }
                break;
                case ETranslate: {
                    check_attributes(node, { "value" });
                    Eigen::Vector3f v = toVector3f(node.attribute("value").value());
                    transform = Eigen::Translation<float, 3>(v.x(), v.y(), v.z()) * transform;
                }
                break;
                case EMatrix: {
                    check_attributes(node, { "value" });
                    std::vector<std::string> tokens = tokenize(node.attribute("value").value());
                    if (tokens.size() != 16)
                        throw NoriException("Expected 16 values");
                    Eigen::Matrix4f matrix;
                    for (int i=0; i<4; ++i)
                        for (int j=0; j<4; ++j)
                            matrix(i, j) = toFloat(tokens[i*4+j]);
                    transform = Eigen::Affine3f(matrix) * transform;
                }
                break;
                case EScale: {
                    check_attributes(node, { "value" });
                    Eigen::Vector3f v = toVector3f(node.attribute("value").value());
                    transform = Eigen::DiagonalMatrix<float, 3>(v) * transform;
                }
                break;
                case ERotate: {
                    check_attributes(node, { "angle", "axis" });
                    float angle = degToRad(toFloat(node.attribute("angle").value()));
                    Eigen::Vector3f axis = toVector3f(node.attribute("axis").value());
                    transform = Eigen::AngleAxis<float>(angle, axis) * transform;
                }
                break;
                case ELookAt: {
                    check_attributes(node, { "origin", "target", "up" });
                    Eigen::Vector3f origin = toVector3f(node.attribute("origin").value());
                    Eigen::Vector3f target = toVector3f(node.attribute("target").value());
                    Eigen::Vector3f up = toVector3f(node.attribute("up").value());

                    Vector3f dir = (target - origin).normalized();
                    Vector3f left = up.normalized().cross(dir);
                    Vector3f newUp = dir.cross(left);

                    Eigen::Matrix4f trafo;
                    trafo << left, newUp, dir, origin,
                          0, 0, 0, 1;

                    transform = Eigen::Affine3f(trafo) * transform;
                }
                break;

                default:
                    throw NoriException("Unhandled element \"%s\"", node.name());
                };
            }
        } catch (const NoriException &e) {
            throw NoriException("Error while parsing \"%s\": %s (at %s)", filename,
                                e.what(), offset(node.offset_debug()));
        }

        return result;
    };

    PropertyList list;
    return parseTag(*doc.begin(), list, EInvalid);
}

template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBox(
  const PointCloudPtr& cloud,
  const pcl::PointIndices& indices,
  const Eigen::Vector3f& plane_normal,
  const Eigen::Vector3f& plane_point,
  Eigen::Vector3f& position,
  Eigen::Quaternion<float>& orientation,
  Eigen::Vector3f& size)
{
  // transform to table coordinate frame and project points on X-Y, save max height
  Eigen::Affine3f tf;
  pcl::getTransformationFromTwoUnitVectorsAndOrigin(plane_normal.unitOrthogonal(), plane_normal, plane_point, tf);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc2d(new pcl::PointCloud<pcl::PointXYZ>);
  float height = 0.0;
  for(std::vector<int>::const_iterator it=indices.indices.begin(); it != indices.indices.end(); ++it)
  {
    Eigen::Vector3f tmp = tf * (*cloud)[*it].getVector3fMap();
    height = std::max<float>(height, fabs(tmp(2)));
    pc2d->push_back(pcl::PointXYZ(tmp(0),tmp(1),0.0));
  }

  // create convex hull of projected points
  #ifdef PCL_MINOR_VERSION >= 6
  pcl::PointCloud<pcl::PointXYZ>::Ptr conv_hull(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ConvexHull<pcl::PointXYZ> chull;
  chull.setDimension(2);
  chull.setInputCloud(pc2d);
  chull.reconstruct(*conv_hull);
  #endif

  /*for(int i=0; i<conv_hull->size(); ++i)
    std::cout << (*conv_hull)[i].x << "," << (*conv_hull)[i].y << std::endl;*/

  // find the minimal bounding rectangle in 2D and table coordinates
  Eigen::Vector2f p1, p2, p3;
  cob_3d_mapping::MinimalRectangle2D mr2d;
  mr2d.setConvexHull(conv_hull->points);
  mr2d.rotatingCalipers(p2, p1, p3);
  /*std::cout << "BB: \n" << p1[0] << "," << p1[1] <<"\n"
            << p2[0] << "," << p2[1] <<"\n"
            << p3[0] << "," << p3[1] <<"\n ---" << std::endl;*/

  // compute center of rectangle
  position[0] = 0.5f*(p1[0] + p3[0]);
  position[1] = 0.5f*(p1[1] + p3[1]);
  position[2] = 0.0f;
  // transform back
  Eigen::Affine3f inv_tf = tf.inverse();
  position = inv_tf * position;
  // set size of bounding box
  float norm_1 = (p3-p2).norm();
  float norm_2 = (p1-p2).norm();
  // BoundingBox coordinates: X:= main direction, Z:= table normal
  Eigen::Vector3f direction; // y direction
  if (norm_1 < norm_2)
  {
    direction = Eigen::Vector3f(p3[0]-p2[0], p3[1]-p2[1], 0) / (norm_1);
    size[0] = norm_2 * 0.5f;
    size[1] = norm_1 * 0.5f;
  }
  else
  {
    direction = Eigen::Vector3f(p1[0]-p2[0], p1[1]-p2[1], 0) / (norm_2);
    size[0] = norm_1 * 0.5f;
    size[1] = norm_2 * 0.5f;
  }
  size[2] = -height;


  direction = inv_tf.rotation() * direction;
  orientation = pcl::getTransformationFromTwoUnitVectors(direction, plane_normal).rotation(); // (y, z-direction)

  return;
  Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - plane_normal * plane_normal.transpose();
  Eigen::Vector3f xn = M * Eigen::Vector3f::UnitX(); // X-axis project on normal
  Eigen::Vector3f xxn = M * direction;
  float cos_phi = acos(xn.normalized().dot(xxn.normalized())); // angle between xn and main direction
  cos_phi = cos(0.5f * cos_phi);
  float sin_phi = sqrt(1.0f-cos_phi*cos_phi);
  //orientation.w() = cos_phi;
  //orientation.x() = sin_phi * plane_normal(0);
  //orientation.y() = sin_phi * plane_normal(1);
  //orientation.z() = sin_phi * plane_normal(2);
}