std::vector<float>
WorkerStemFit::compute_distances(std::vector<pcl::ModelCoefficients> &cylinders)
{
std::vector<float> distances ( _cloud->points.size () );
std::fill ( distances.begin (), distances.end (), 0.5 );
pcl::octree::OctreePointCloudSearch<PointI> octree ( 0.02f );
octree.setInputCloud ( _cloud );
octree.addPointsFromInputCloud ();
for (size_t i = 0; i < cylinders.size(); i++) {
pcl::ModelCoefficients cylinder = cylinders.at(i);
std::vector<int> indices = indexOfPointsNearCylinder ( octree, cylinder );
for ( size_t i = 0; i < indices.size (); i++ ) {
PointI point = _cloud->points[indices[i]];
simpleTree::Cylinder cyl (cylinder);
float dist = cyl.distToPoint ( point );
if ( std::abs ( dist ) < std::abs ( distances[indices[i]] ) ) {
distances[indices[i]] = dist;
}
}
}
return distances;
}
void CylinderRule::execute(Grammar &grammar, Context &context, Shape &shape) {
if (shape.getShapeType() != ST_Prism) {
// La règle ne s'applique que sur un prisme
throw std::runtime_error("build_cylinder can only be used on Prism primitives");
}
Prism const& prism = static_cast<Prism const&>(shape);
std::vector<glm::vec2> const &points = prism.poly.getPoints();
const int n = points.size();
// Find the centroid
glm::vec2 centroid;
for (int i = 0; i < n; i++) {
centroid += points[i];
}
centroid /= n;
glm::vec3 centroid3d(centroid.x, 0, centroid.y);
centroid3d = centroid3d + glm::vec3(0, context.terrain->getHeight(centroid), 0);
// Build a cylinder of center 'centroid' of radius 'radius' and height 'height'
float radius = GRandom.RandomNumber(radius_min, radius_max);
float height = GRandom.RandomNumber(height_min, height_max);
Circle2D circle(radius, centroid3d);
Cylinder cyl(circle, height);
// Successeurs
if (successors.size() != 1) {
throw std::runtime_error("build_cylinder operation must have only one successor");
}
CSymbol *succ = *successors.begin();
grammar.resolve(succ, context, cyl);
}
int main()
{
Cylinder cyl(10.0, 8.0), cyl2;
cyl2 = ret(); //call on Cylinder& Cylinder::operator=(Cylinder &&cyl)
std::cout << "Volume of the cylinder is " << cyl2.volume() << std::endl;
return 0;
}
static void *unformatted_write_raw(
struct disk *d, unsigned int tracknr, struct stream *s)
{
struct disk_info *di = d->di;
struct track_info *ti = &di->track[tracknr];
unsigned int scan_bits = 0, bad = 0, nr_zero = 0;
unsigned int lat = s->latency, clk = s->clock;
unsigned int bad_sectors = 0, nr_sectors = 0;
/* Scan for bit sequences that break the MFM encoding rules.
* Random noise will obviously do this a *lot*. */
while (stream_next_bit(s) != -1) {
if (s->word & 1) {
unsigned int new_clk = (s->latency - lat) / (nr_zero + 1);
int delta = new_clk - clk;
if (delta < 0)
delta = -delta;
if (((delta*100)/clk) > CLOCK_JITTER_THRESH)
bad++;
clk = new_clk;
lat = s->latency;
nr_zero = 0;
} else if (++nr_zero > 3) {
bad++;
}
if (++scan_bits >= SCAN_SECTOR_BITS) {
if (bad >= SECTOR_BAD_THRESH)
bad_sectors++;
nr_sectors++;
bad = scan_bits = 0;
}
}
if (bad_sectors < nr_sectors) {
unsigned int pc = (bad_sectors*1000)/nr_sectors;
if ((pc/10) <= 90)
return NULL;
printf("*** T%u.%u: Almost certainly unformatted/empty (%u.%u%%)\n",
cyl(tracknr), hd(tracknr), pc/10, pc%10);
}
ti->total_bits = TRK_WEAK;
return memalloc(0); /* dummy */
}
primitives primitives::cylinder_z(double radius_bottom, double radius_top,
double z_bottom, double z_top,
double alpha, const texture& tex,
double u_scal, unsigned nr_segs, bool inside)
{
primitives cyl(GL_QUAD_STRIP, (nr_segs+1)*2, tex);
color col(255, 255, 255, 255);
double us = u_scal / nr_segs;
for (unsigned i = 0; i <= nr_segs; ++i) {
double a = -2*M_PI*i/nr_segs;
double sa = sin(a);
double ca = cos(a);
if (inside) ca = -ca;
col.a = Uint8(128 + 127*alpha);
cyl.colors[2*i] = col;
col.a = Uint8(255*alpha);
cyl.colors[2*i+1] = col;
cyl.texcoords[2*i] = vector2f(i * us, 1);
cyl.texcoords[2*i+1] = vector2f(i * us, 0);
cyl.vertices[2*i] = vector3f(radius_bottom * ca, radius_bottom * sa, z_bottom);
cyl.vertices[2*i+1] = vector3f(radius_top * ca, radius_top * sa, z_top);
}
return cyl;
}
TEST(WorldDiff, TrackChanges)
{
collision_detection::WorldPtr world(new collision_detection::World);
collision_detection::WorldDiff diff1(world);
collision_detection::WorldDiff diff2;
collision_detection::WorldDiff::const_iterator it;
EXPECT_EQ(0, diff1.getChanges().size());
EXPECT_EQ(0, diff2.getChanges().size());
// Create some shapes
shapes::ShapePtr ball(new shapes::Sphere(1.0));
shapes::ShapePtr box(new shapes::Box(1,2,3));
shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
world->addToObject("obj1",
ball,
Eigen::Affine3d::Identity());
EXPECT_EQ(1, diff1.getChanges().size());
EXPECT_EQ(0, diff2.getChanges().size());
it = diff1.getChanges().find("obj1");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
it = diff1.getChanges().find("xyz");
EXPECT_EQ(diff1.end(), it);
world->addToObject("obj2",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(2, diff1.getChanges().size());
EXPECT_EQ(0, diff2.getChanges().size());
it = diff1.getChanges().find("obj2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
world->addToObject("obj2",
cyl,
Eigen::Affine3d::Identity());
EXPECT_EQ(2, diff1.getChanges().size());
EXPECT_EQ(0, diff2.getChanges().size());
it = diff1.getChanges().find("obj2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
diff2.reset(world);
bool move_ok = world->moveShapeInObject(
"obj2",
cyl,
Eigen::Affine3d(Eigen::Translation3d(0,0,1)));
EXPECT_TRUE(move_ok);
EXPECT_EQ(2, diff1.getChanges().size());
EXPECT_EQ(1, diff2.getChanges().size());
it = diff1.getChanges().find("obj2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE |
collision_detection::World::MOVE_SHAPE,
it->second);
it = diff2.getChanges().find("obj2");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::MOVE_SHAPE,
it->second);
EXPECT_EQ("obj2", it->first);
diff1.reset(world);
EXPECT_EQ(0, diff1.getChanges().size());
EXPECT_EQ(1, diff2.getChanges().size());
it = diff1.getChanges().find("obj2");
EXPECT_EQ(diff1.end(), it);
world->addToObject("obj3",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(1, diff1.getChanges().size());
EXPECT_EQ(2, diff2.getChanges().size());
world->addToObject("obj3",
cyl,
Eigen::Affine3d::Identity());
world->addToObject("obj3",
ball,
Eigen::Affine3d::Identity());
EXPECT_EQ(1, diff1.getChanges().size());
EXPECT_EQ(2, diff2.getChanges().size());
diff1.reset();
move_ok = world->moveShapeInObject(
"obj3",
cyl,
Eigen::Affine3d(Eigen::Translation3d(0,0,2)));
EXPECT_TRUE(move_ok);
EXPECT_EQ(0, diff1.getChanges().size());
EXPECT_EQ(2, diff2.getChanges().size());
diff1.reset(world);
world->removeObject("obj2");
EXPECT_EQ(1, diff1.getChanges().size());
EXPECT_EQ(2, diff2.getChanges().size());
it = diff1.getChanges().find("obj2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff2.getChanges().find("obj2");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
world->removeShapeFromObject("obj3", cyl);
it = diff1.getChanges().find("obj3");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
it->second);
it = diff2.getChanges().find("obj3");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE |
collision_detection::World::MOVE_SHAPE |
collision_detection::World::REMOVE_SHAPE,
it->second);
world->removeShapeFromObject("obj3", box);
it = diff1.getChanges().find("obj3");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
it->second);
it = diff2.getChanges().find("obj3");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE |
collision_detection::World::MOVE_SHAPE |
collision_detection::World::REMOVE_SHAPE,
it->second);
move_ok = world->moveShapeInObject(
"obj3",
ball,
Eigen::Affine3d(Eigen::Translation3d(0,0,3)));
EXPECT_TRUE(move_ok);
it = diff1.getChanges().find("obj3");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::REMOVE_SHAPE |
collision_detection::World::MOVE_SHAPE,
it->second);
it = diff2.getChanges().find("obj3");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE |
collision_detection::World::MOVE_SHAPE |
collision_detection::World::REMOVE_SHAPE,
it->second);
world->removeShapeFromObject("obj3", ball);
it = diff1.getChanges().find("obj3");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff2.getChanges().find("obj3");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
}
TEST(WorldDiff, SetWorld)
{
collision_detection::WorldPtr world1(new collision_detection::World);
collision_detection::WorldPtr world2(new collision_detection::World);
collision_detection::WorldDiff diff1(world1);
collision_detection::WorldDiff diff1b(world1);
collision_detection::WorldDiff diff2(world2);
collision_detection::WorldDiff::const_iterator it;
shapes::ShapePtr ball(new shapes::Sphere(1.0));
shapes::ShapePtr box(new shapes::Box(1,2,3));
shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
world1->addToObject("objA1",
ball,
Eigen::Affine3d::Identity());
world1->addToObject("objA2",
ball,
Eigen::Affine3d::Identity());
world1->addToObject("objA3",
ball,
Eigen::Affine3d::Identity());
world2->addToObject("objB1",
box,
Eigen::Affine3d::Identity());
world2->addToObject("objB2",
box,
Eigen::Affine3d::Identity());
world2->addToObject("objB3",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(3, diff1.getChanges().size());
EXPECT_EQ(3, diff1b.getChanges().size());
EXPECT_EQ(3, diff2.getChanges().size());
diff1b.clearChanges();
EXPECT_EQ(3, diff1.getChanges().size());
EXPECT_EQ(0, diff1b.getChanges().size());
EXPECT_EQ(3, diff2.getChanges().size());
diff1.setWorld(world2);
EXPECT_EQ(6, diff1.getChanges().size());
EXPECT_EQ(0, diff1b.getChanges().size());
EXPECT_EQ(3, diff2.getChanges().size());
it = diff1.getChanges().find("objA1");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1.getChanges().find("objA2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1.getChanges().find("objA2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1.getChanges().find("objB1");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
it = diff1.getChanges().find("objB2");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
it = diff1.getChanges().find("objB3");
EXPECT_NE(diff1.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
diff1b.setWorld(world2);
EXPECT_EQ(6, diff1.getChanges().size());
EXPECT_EQ(6, diff1b.getChanges().size());
EXPECT_EQ(3, diff2.getChanges().size());
it = diff1b.getChanges().find("objA1");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1b.getChanges().find("objA2");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1b.getChanges().find("objA2");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::DESTROY,
it->second);
it = diff1b.getChanges().find("objB1");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
it = diff1b.getChanges().find("objB2");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
it = diff1b.getChanges().find("objB3");
EXPECT_NE(diff1b.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE,
it->second);
world1->addToObject("objC",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(6, diff1.getChanges().size());
EXPECT_EQ(6, diff1b.getChanges().size());
EXPECT_EQ(3, diff2.getChanges().size());
world2->addToObject("objC",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(7, diff1.getChanges().size());
EXPECT_EQ(7, diff1b.getChanges().size());
EXPECT_EQ(4, diff2.getChanges().size());
diff2.setWorld(world1);
EXPECT_EQ(7, diff1.getChanges().size());
EXPECT_EQ(7, diff1b.getChanges().size());
EXPECT_EQ(7, diff2.getChanges().size());
it = diff2.getChanges().find("objC");
EXPECT_NE(diff2.end(), it);
EXPECT_EQ(collision_detection::World::CREATE |
collision_detection::World::ADD_SHAPE |
collision_detection::World::DESTROY,
it->second);
world1->addToObject("objD",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(7, diff1.getChanges().size());
EXPECT_EQ(7, diff1b.getChanges().size());
EXPECT_EQ(8, diff2.getChanges().size());
world2->addToObject("objE",
box,
Eigen::Affine3d::Identity());
EXPECT_EQ(8, diff1.getChanges().size());
EXPECT_EQ(8, diff1b.getChanges().size());
EXPECT_EQ(8, diff2.getChanges().size());
}
void modelCB(const pcl::ModelCoefficients::ConstPtr& msg)
{
if (msg->values.size() < 7)
{
ROS_DEBUG("Not enough coefficients to be a cylinder: %i.",
(int)msg->values.size());
return;
}
// 1. Create the target pose.
// Cylinder structure: {{point_on_axis}, {axis_direction}, radius}.
// The axis direction is normalized.
//
// TODO: Figure out where the point is relative to the cylinder,
// e.g the base, CoG, ...
// Cylinder origin, seems to be CoG.
tf::Vector3 p(
msg->values[0],
msg->values[1],
msg->values[2]);
// Cylinder vertical axis.
tf::Vector3 cyl(
msg->values[3],
msg->values[4],
msg->values[5]);
// TODO: Make sure it points to the sky (needs a reference).
// The test is currently baked knowing the fact that the Y axis
// of the camera points to the ground.
if (cyl.y() > 0)
cyl *= -1.0;
// Cummulative average on the position, axis and height.
avg_count_ += 1.0;
p = old_p_ + ((p - old_p_) / avg_count_);
cyl = old_cyl_ + ((cyl - old_cyl_) / avg_count_);
cyl.normalize();
old_p_ = p;
old_cyl_ = cyl;
// Orientation:
// - X points to the thumb.
// - Y is perpendicular to the grasp plane.
// - Z is the wrist rotation axis, and should point outward
// the camera.
// We generate Z to point toward the ground, so the inverse of
// the cylinder axis.
// We generate X with the cross product of the view axis (the
// cylinder's position) and the Y axis so that it points to the
// left.
// Y axis is the cross product of Z and X.
tf::Vector3 view = p.normalized();
tf::Vector3 oz = -1.0 * cyl;
tf::Vector3 ox = view.cross(oz);
tf::Vector3 oy = oz.cross(ox);
// !! OLD SETUP !!
// We generate X with the cross product of the cylinder axis (Y)
// and the view vector and should point to the left.
// Z is then produced with the cross product of X and Y.
// tf::Vector3 view = p.normalized();
// tf::Vector3 oy = cyl;
// tf::Vector3 ox = oy.cross(view).normalized();
// tf::Vector3 oz = ox.cross(oy);
// Convert this into a quaternion from the basis matrix.
tf::Matrix3x3 basis(
ox.x(), oy.x(), oz.x(),
ox.y(), oy.y(), oz.y(),
ox.z(), oy.z(), oz.z());
// Convert to the pose message.
// Target pose: half a cylinder height back from the center, minus
// 3cm to give enough grip to the fingers.
tf::Vector3 target_p = p - ((0.5 * cyl_height_ - 0.03) * oz);
target_pose_.header = msg->header;
tf::pointTFToMsg(target_p, target_pose_.pose.position);
tf::Quaternion q;
basis.getRotation(q);
tf::quaternionTFToMsg(q, target_pose_.pose.orientation);
// 1. Create the "toward" pose (back a full cylinder height).
tf::Vector3 toward_pos = p - (cyl_height_ * oz);
toward_pose_.header = target_pose_.header;
tf::pointTFToMsg(toward_pos, toward_pose_.pose.position);
toward_pose_.pose.orientation = target_pose_.pose.orientation;
// 2. Create the "lifted" pose (same as "toward").
tf::Vector3 lifted_pos = toward_pos;
lifted_pose_.header = target_pose_.header;
tf::pointTFToMsg(lifted_pos, lifted_pose_.pose.position);
lifted_pose_.pose.orientation = target_pose_.pose.orientation;
// For diagnostic purposes:
pub_toward_.publish(toward_pose_);
pub_target_.publish(target_pose_);
pub_lifted_.publish(lifted_pose_);
}