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 ();
}
void stateCallback(const nav_msgs::Odometry::ConstPtr& state)
{
if(mode==1){
vel_des[0]=rc_vx;
vel_des[1]=rc_vy;
vel_des[2]=0;
pos_des[1]=0;
pos_des[2]=rc_z;
}
else if(mode==2)
{
double cur_t = ros::Time::now().toSec();
double t = cur_t-init_t;
vel_des[2]=0;
if(t<t1){
vel_des[0]=ax*t;
vel_des[1]=ay*t;
acc_des[0]=ax;
acc_des[1]=ay;
acc_des[2]=0;
}
else if(t<des_t-t1&&t>=t1){
vel_des[0]=des_vx;
vel_des[1]=des_vy;
acc_des = Eigen::Vector3f::Zero();
}
else if(t>=des_t-t1&&t<des_t){
vel_des[0] = des_vx-ax*(t+t1-des_t);
vel_des[1] = des_vy-ay*(t+t1-des_t);
acc_des[0] = -ax;
acc_des[1] = -ay;
acc_des[2] = 0;
}
else if(t>=des_t){
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;
}
else if(mode==3)
{
double cur_t = ros::Time::now().toSec();
double t = cur_t-init_t;
vel_des[2]=0;
if(t<t1){
vel_des[0]=ax*t;
vel_des[1]=ay*t;
acc_des[0]=ax;
acc_des[1]=ay;
acc_des[2]=0;
}
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);
}
void SilhouetteExtractor::computeVisibleFrontFacingStatus()
{
int terrain_width = terrain_->width();
int terrain_height = terrain_->height();
delete front_facing_;
front_facing_ = new FacingMode [(terrain_width-1)*(terrain_height-1)];
bool use_intersections = true ;
if (!use_intersections)
{
setupPixelBuffer();
pixelbuffer_->makeCurrent();
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
GLfloat lightpos[] = {.5, 1., 1., 0.};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
drawTerrain();
updateMatrices();
saveBuffer("test_dpth.png");
pixelbuffer_->makeCurrent();
}
double begin = omp_get_wtime();
int i = 0;
#pragma omp parallel for private(i)
for (int j = 0; j < terrain_->height()-1; ++j)
for (i = 0; i < terrain_->width()-1; ++i)
{
front_facing_[j*(terrain_width-1)+i] = kInvisible;
Eigen::Vector3f center = getFaceCentroid(i, j);
Eigen::Vector3f projector = center - camera_info_.position;
//projector = camera_info_.direction;
float theta = acos(camera_info_.direction.normalized().dot(projector.normalized()));
if (theta > camera_info_.fov_in_rads/2)
continue;
front_facing_[j*(terrain_width-1)+i] = kBackFacing;
if (terrain_->getGridNormal(i, j).dot(projector) <= -FLT_EPSILON)
{
if (use_intersections)
{
if (checkVisibility(center))
front_facing_[j*(terrain_width-1)+i] = kFrontFacing;
} else
{
Eigen::Vector3d window_coords;
gluProject(center[0], center[1], center[2],
modelview_matrix_, projection_matrix_, viewport_,
&window_coords[0], &window_coords[1], &window_coords[2]);
if (window_coords[0] < 0 || window_coords[1] < 0 || window_coords[0] >= width() || window_coords[1] >= height())
continue;
float depth = 0.0;
glReadPixels(window_coords[0], window_coords[1], 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &depth);
if (std::abs(depth-window_coords[2]) < 1e-3)
front_facing_[j*(terrain_width-1)+i] = kFrontFacing;
}
}
}
double end = omp_get_wtime();
double elapsed_secs = double(end - begin);
fprintf(stdout, "Elapsed time for checking front/back facing: %.2f secs\n", elapsed_secs);
fprintf(stdout, "Num of threads: %d threads\n", omp_get_thread_num());
fflush(stdout);
if (pixelbuffer_)
{
pixelbuffer_->doneCurrent();
cleanupPixelBuffer();
}
}
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;
}
NORI_NAMESPACE_BEGIN
NoriObject *loadFromXML(const std::string &filename) {
/* Load the XML file using 'pugi' (a tiny self-contained XML parser implemented in C++) */
pugi::xml_document doc;
pugi::xml_parse_result result = doc.load_file(filename.c_str());
/* Helper function: map a position offset in bytes to a more readable row/column value */
auto offset = [&](ptrdiff_t pos) -> std::string {
std::fstream is(filename);
char buffer[1024];
int line = 0, linestart = 0, offset = 0;
while (is.good()) {
is.read(buffer, sizeof(buffer));
for (int i = 0; i < is.gcount(); ++i) {
if (buffer[i] == '\n') {
if (offset + i >= pos)
return tfm::format("row %i, col %i", line + 1, pos - linestart);
++line;
linestart = offset + i;
}
}
offset += (int) is.gcount();
}
return "byte offset " + std::to_string(pos);
};
if (!result) /* There was a parser / file IO error */
throw NoriException("Error while parsing \"%s\": %s (at %s)", filename, result.description(), offset(result.offset));
/* Set of supported XML tags */
enum ETag {
/* Object classes */
EScene = NoriObject::EScene,
EMesh = NoriObject::EMesh,
EBSDF = NoriObject::EBSDF,
ETEXTURE = NoriObject::ETEXTURE,
EPERLIN = NoriObject::EPERLIN,
EMIXTEXTURE = NoriObject::EMIXTEXTURE,
EMIXBUMPMAP = NoriObject::EMIXBUMPMAP,
EBUMPMAP = NoriObject::EBUMPMAP,
EPhaseFunction = NoriObject::EPhaseFunction,
EEmitter = NoriObject::EEmitter,
EMedium = NoriObject::EMedium,
EVolume = NoriObject::EVolume,
ECamera = NoriObject::ECamera,
EIntegrator = NoriObject::EIntegrator,
ESampler = NoriObject::ESampler,
ETest = NoriObject::ETest,
EReconstructionFilter = NoriObject::EReconstructionFilter,
/* Properties */
EBoolean = NoriObject::EClassTypeCount,
EInteger,
EFloat,
EString,
EPoint,
EVector,
EColor,
ETransform,
ETranslate,
EMatrix,
ERotate,
EScale,
ELookAt,
EInvalid
};
/* Create a mapping from tag names to tag IDs */
std::map<std::string, ETag> tags;
tags["scene"] = EScene;
tags["mesh"] = EMesh;
tags["bsdf"] = EBSDF;
tags["texture"] = ETEXTURE;
tags["bumpmap"] = EBUMPMAP;
tags["perlin"] = EPERLIN;
tags["mixTexture"] = EMIXTEXTURE;
tags["mixBumpmap"] = EMIXBUMPMAP;
tags["bumpmap"] = EBUMPMAP;
tags["emitter"] = EEmitter;
tags["camera"] = ECamera;
tags["medium"] = EMedium;
tags["volume"] = EVolume;
tags["phase"] = EPhaseFunction;
tags["integrator"] = EIntegrator;
tags["sampler"] = ESampler;
tags["rfilter"] = EReconstructionFilter;
tags["test"] = ETest;
tags["boolean"] = EBoolean;
tags["integer"] = EInteger;
tags["float"] = EFloat;
tags["string"] = EString;
tags["point"] = EPoint;
tags["vector"] = EVector;
tags["color"] = EColor;
tags["transform"] = ETransform;
tags["translate"] = ETranslate;
tags["matrix"] = EMatrix;
tags["rotate"] = ERotate;
tags["scale"] = EScale;
tags["lookat"] = ELookAt;
/* Helper function to check if attributes are fully specified */
auto check_attributes = [&](const pugi::xml_node &node, std::set<std::string> attrs) {
for (auto attr : node.attributes()) {
auto it = attrs.find(attr.name());
if (it == attrs.end())
throw NoriException("Error while parsing \"%s\": unexpected attribute \"%s\" in \"%s\" at %s",
filename, attr.name(), node.name(), offset(node.offset_debug()));
attrs.erase(it);
}
if (!attrs.empty())
throw NoriException("Error while parsing \"%s\": missing attribute \"%s\" in \"%s\" at %s",
filename, *attrs.begin(), node.name(), offset(node.offset_debug()));
};
Eigen::Affine3f transform;
/* Helper function to parse a Nori XML node (recursive) */
std::function<NoriObject *(pugi::xml_node &, PropertyList &, int)> parseTag = [&](
pugi::xml_node &node, PropertyList &list, int parentTag) -> NoriObject * {
/* Skip over comments */
if (node.type() == pugi::node_comment || node.type() == pugi::node_declaration)
return nullptr;
if (node.type() != pugi::node_element)
throw NoriException(
"Error while parsing \"%s\": unexpected content at %s",
filename, offset(node.offset_debug()));
/* Look up the name of the current element */
auto it = tags.find(node.name());
if (it == tags.end())
throw NoriException("Error while parsing \"%s\": unexpected tag \"%s\" at %s",
filename, node.name(), offset(node.offset_debug()));
int tag = it->second;
/* Perform some safety checks to make sure that the XML tree really makes sense */
bool hasParent = parentTag != EInvalid;
bool parentIsObject = hasParent && parentTag < NoriObject::EClassTypeCount;
bool currentIsObject = tag < NoriObject::EClassTypeCount;
bool parentIsTransform = parentTag == ETransform;
bool currentIsTransformOp = tag == ETranslate || tag == ERotate || tag == EScale || tag == ELookAt || tag == EMatrix;
if (!hasParent && !currentIsObject)
throw NoriException("Error while parsing \"%s\": root element \"%s\" must be a Nori object (at %s)",
filename, node.name(), offset(node.offset_debug()));
if (parentIsTransform != currentIsTransformOp)
throw NoriException("Error while parsing \"%s\": transform nodes "
"can only contain transform operations (at %s)",
filename, offset(node.offset_debug()));
if (hasParent && !parentIsObject && !(parentIsTransform && currentIsTransformOp))
throw NoriException("Error while parsing \"%s\": node \"%s\" requires a Nori object as parent (at %s)",
filename, node.name(), offset(node.offset_debug()));
if (tag == EScene)
node.append_attribute("type") = "scene";
else if (tag == ETransform)
transform.setIdentity();
PropertyList propList;
std::vector<NoriObject *> children;
for (pugi::xml_node &ch: node.children()) {
NoriObject *child = parseTag(ch, propList, tag);
if (child)
children.push_back(child);
}
NoriObject *result = nullptr;
try {
if (currentIsObject) {
check_attributes(node, { "type" });
/* This is an object, first instantiate it */
result = NoriObjectFactory::createInstance(
node.attribute("type").value(),
propList
);
if (result->getClassType() != (int) tag) {
throw NoriException(
"Unexpectedly constructed an object "
"of type <%s> (expected type <%s>): %s",
NoriObject::classTypeName(result->getClassType()),
NoriObject::classTypeName((NoriObject::EClassType) tag),
result->toString());
}
/* Add all children */
for (auto ch: children) {
result->addChild(ch);
ch->setParent(result);
}
/* Activate / configure the object */
result->activate();
} else {
/* This is a property */
switch (tag) {
case EString: {
check_attributes(node, { "name", "value" });
list.setString(node.attribute("name").value(), node.attribute("value").value());
}
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);
}
Plane::Plane(Eigen::Vector3f normal, Eigen::Vector3f p) :
normal_(normal.normalized()), d_(- normal.dot(p) / normal.norm())
{
initializeCoordinates();
}
BlockFitter::Result BlockFitter::
go() {
Result result;
result.mSuccess = false;
if (mCloud->size() < 100) return result;
// voxelize
LabeledCloud::Ptr cloud(new LabeledCloud());
pcl::VoxelGrid<pcl::PointXYZL> voxelGrid;
voxelGrid.setInputCloud(mCloud);
voxelGrid.setLeafSize(mDownsampleResolution, mDownsampleResolution,
mDownsampleResolution);
voxelGrid.filter(*cloud);
for (int i = 0; i < (int)cloud->size(); ++i) cloud->points[i].label = i;
if (mDebug) {
std::cout << "Original cloud size " << mCloud->size() << std::endl;
std::cout << "Voxelized cloud size " << cloud->size() << std::endl;
pcl::io::savePCDFileBinary("cloud_full.pcd", *cloud);
}
if (cloud->size() < 100) return result;
// pose
cloud->sensor_origin_.head<3>() = mOrigin;
cloud->sensor_origin_[3] = 1;
Eigen::Vector3f rz = mLookDir;
Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ());
Eigen::Vector3f ry = rz.cross(rx);
Eigen::Matrix3f rotation;
rotation.col(0) = rx.normalized();
rotation.col(1) = ry.normalized();
rotation.col(2) = rz.normalized();
Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
pose.linear() = rotation;
pose.translation() = mOrigin;
// ground removal
if (mRemoveGround) {
Eigen::Vector4f groundPlane;
// filter points
float minZ = mMinGroundZ;
float maxZ = mMaxGroundZ;
if ((minZ > 10000) && (maxZ > 10000)) {
std::vector<float> zVals(cloud->size());
for (int i = 0; i < (int)cloud->size(); ++i) {
zVals[i] = cloud->points[i].z;
}
std::sort(zVals.begin(), zVals.end());
minZ = zVals[0]-0.1;
maxZ = minZ + 0.5;
}
LabeledCloud::Ptr tempCloud(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
const Eigen::Vector3f& p = cloud->points[i].getVector3fMap();
if ((p[2] < minZ) || (p[2] > maxZ)) continue;
tempCloud->push_back(cloud->points[i]);
}
// downsample
voxelGrid.setInputCloud(tempCloud);
voxelGrid.setLeafSize(0.1, 0.1, 0.1);
voxelGrid.filter(*tempCloud);
if (tempCloud->size() < 100) return result;
// find ground plane
std::vector<Eigen::Vector3f> pts(tempCloud->size());
for (int i = 0; i < (int)tempCloud->size(); ++i) {
pts[i] = tempCloud->points[i].getVector3fMap();
}
const float kGroundPlaneDistanceThresh = 0.01; // TODO: param
PlaneFitter planeFitter;
planeFitter.setMaxDistance(kGroundPlaneDistanceThresh);
planeFitter.setRefineUsingInliers(true);
auto res = planeFitter.go(pts);
groundPlane = res.mPlane;
if (groundPlane[2] < 0) groundPlane = -groundPlane;
if (mDebug) {
std::cout << "dominant plane: " << groundPlane.transpose() << std::endl;
std::cout << " inliers: " << res.mInliers.size() << std::endl;
}
if (std::acos(groundPlane[2]) > 30*M_PI/180) {
std::cout << "error: ground plane not found!" << std::endl;
std::cout << "proceeding with full segmentation (may take a while)..." <<
std::endl;
}
else {
// compute convex hull
result.mGroundPlane = groundPlane;
{
tempCloud.reset(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
Eigen::Vector3f p = cloud->points[i].getVector3fMap();
float dist = groundPlane.head<3>().dot(p) + groundPlane[3];
if (std::abs(dist) > kGroundPlaneDistanceThresh) continue;
p -= (groundPlane.head<3>()*dist);
pcl::PointXYZL cloudPt;
cloudPt.getVector3fMap() = p;
tempCloud->push_back(cloudPt);
}
pcl::ConvexHull<pcl::PointXYZL> chull;
pcl::PointCloud<pcl::PointXYZL> hull;
chull.setInputCloud(tempCloud);
chull.reconstruct(hull);
result.mGroundPolygon.resize(hull.size());
for (int i = 0; i < (int)hull.size(); ++i) {
result.mGroundPolygon[i] = hull[i].getVector3fMap();
}
}
// remove points below or near ground
tempCloud.reset(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
Eigen::Vector3f p = cloud->points[i].getVector3fMap();
float dist = p.dot(groundPlane.head<3>()) + groundPlane[3];
if ((dist < mMinHeightAboveGround) ||
(dist > mMaxHeightAboveGround)) continue;
float range = (p-mOrigin).norm();
if (range > mMaxRange) continue;
tempCloud->push_back(cloud->points[i]);
}
std::swap(tempCloud, cloud);
if (mDebug) {
std::cout << "Filtered cloud size " << cloud->size() << std::endl;
}
}
}
// normal estimation
auto t0 = std::chrono::high_resolution_clock::now();
if (mDebug) {
std::cout << "computing normals..." << std::flush;
}
RobustNormalEstimator normalEstimator;
normalEstimator.setMaxEstimationError(0.01);
normalEstimator.setRadius(0.1);
normalEstimator.setMaxCenterError(0.02);
normalEstimator.setMaxIterations(100);
NormalCloud::Ptr normals(new NormalCloud());
normalEstimator.go(cloud, *normals);
if (mDebug) {
auto t1 = std::chrono::high_resolution_clock::now();
auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;
}
// filter non-horizontal points
const float maxNormalAngle = mMaxAngleFromHorizontal*M_PI/180;
LabeledCloud::Ptr tempCloud(new LabeledCloud());
NormalCloud::Ptr tempNormals(new NormalCloud());
for (int i = 0; i < (int)normals->size(); ++i) {
const auto& norm = normals->points[i];
Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z);
float angle = std::acos(normal[2]);
if (angle > maxNormalAngle) continue;
tempCloud->push_back(cloud->points[i]);
tempNormals->push_back(normals->points[i]);
}
std::swap(tempCloud, cloud);
std::swap(tempNormals, normals);
if (mDebug) {
std::cout << "Horizontal points remaining " << cloud->size() << std::endl;
pcl::io::savePCDFileBinary("cloud.pcd", *cloud);
pcl::io::savePCDFileBinary("robust_normals.pcd", *normals);
}
// plane segmentation
t0 = std::chrono::high_resolution_clock::now();
if (mDebug) {
std::cout << "segmenting planes..." << std::flush;
}
PlaneSegmenter segmenter;
segmenter.setData(cloud, normals);
segmenter.setMaxError(0.05);
segmenter.setMaxAngle(5);
segmenter.setMinPoints(100);
PlaneSegmenter::Result segmenterResult = segmenter.go();
if (mDebug) {
auto t1 = std::chrono::high_resolution_clock::now();
auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;
std::ofstream ofs("labels.txt");
for (const int lab : segmenterResult.mLabels) {
ofs << lab << std::endl;
}
ofs.close();
ofs.open("planes.txt");
for (auto it : segmenterResult.mPlanes) {
auto& plane = it.second;
ofs << it.first << " " << plane.transpose() << std::endl;
}
ofs.close();
}
// create point clouds
std::unordered_map<int,std::vector<Eigen::Vector3f>> cloudMap;
for (int i = 0; i < (int)segmenterResult.mLabels.size(); ++i) {
int label = segmenterResult.mLabels[i];
if (label <= 0) continue;
cloudMap[label].push_back(cloud->points[i].getVector3fMap());
}
struct Plane {
MatrixX3f mPoints;
Eigen::Vector4f mPlane;
};
std::vector<Plane> planes;
planes.reserve(cloudMap.size());
for (auto it : cloudMap) {
int n = it.second.size();
Plane plane;
plane.mPoints.resize(n,3);
for (int i = 0; i < n; ++i) plane.mPoints.row(i) = it.second[i];
plane.mPlane = segmenterResult.mPlanes[it.first];
planes.push_back(plane);
}
std::vector<RectangleFitter::Result> results;
for (auto& plane : planes) {
RectangleFitter fitter;
fitter.setDimensions(mBlockDimensions.head<2>());
fitter.setAlgorithm((RectangleFitter::Algorithm)mRectangleFitAlgorithm);
fitter.setData(plane.mPoints, plane.mPlane);
auto result = fitter.go();
results.push_back(result);
}
if (mDebug) {
std::ofstream ofs("boxes.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
for (auto& p : res.mPolygon) {
ofs << i << " " << p.transpose() << std::endl;
}
}
ofs.close();
ofs.open("hulls.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
for (auto& p : res.mConvexHull) {
ofs << i << " " << p.transpose() << std::endl;
}
}
ofs.close();
ofs.open("poses.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
auto transform = res.mPose;
ofs << transform.matrix() << std::endl;
}
ofs.close();
}
for (int i = 0; i < (int)results.size(); ++i) {
const auto& res = results[i];
if (mBlockDimensions.head<2>().norm() > 1e-5) {
float areaRatio = mBlockDimensions.head<2>().prod()/res.mConvexArea;
if ((areaRatio < mAreaThreshMin) ||
(areaRatio > mAreaThreshMax)) continue;
}
Block block;
block.mSize << res.mSize[0], res.mSize[1], mBlockDimensions[2];
block.mPose = res.mPose;
block.mPose.translation() -=
block.mPose.rotation().col(2)*mBlockDimensions[2]/2;
block.mHull = res.mConvexHull;
result.mBlocks.push_back(block);
}
if (mDebug) {
std::cout << "Surviving blocks: " << result.mBlocks.size() << std::endl;
}
result.mSuccess = true;
return result;
}
cPlane::cPlane(const Eigen::Vector3f& n, float d): mNormal(n.normalized()), mD(d)
{
}
cPlane::cPlane(const Eigen::Vector3f& n, float d, const sMaterial& mat): mNormal(n.normalized()), mD(d), aWorldObject(mat)
{
}