double EuclidDistHeuristic::computeDistance(
    const Affine3& a,
    const Affine3& b) const
{
    auto sqrd = [](double d) { return d * d; };

    Vector3 diff = b.translation() - a.translation();

    double dp2 =
            m_x_coeff * sqrd(diff.x()) +
            m_y_coeff * sqrd(diff.y()) +
            m_z_coeff * sqrd(diff.z());

    Quaternion qb(b.rotation());
    Quaternion qa(a.rotation());

    double dot = qa.dot(qb);
    if (dot < 0.0) {
        qb = Quaternion(-qb.w(), -qb.x(), -qb.y(), -qb.z());
        dot = qa.dot(qb);
    }

    double dr2 = angles::normalize_angle(2.0 * std::acos(dot));
    dr2 *= (m_rot_coeff * dr2);

    SMPL_DEBUG_NAMED(LOG, "Compute Distance: sqrt(%f + %f)", dp2, dr2);

    return std::sqrt(dp2 + dr2);
}
int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
    if (state_id == planningSpace()->getGoalStateID()) {
        return 0;
    }

    if (m_pose_ext) {
        Affine3 p;
        if (!m_pose_ext->projectToPose(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;

        const double dist = computeDistance(p, goal_pose);

        const int h = FIXED_POINT_RATIO * dist;

        double Y, P, R;
        angles::get_euler_zyx(p.rotation(), Y, P, R);
        SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);

        return h;
    } else if (m_point_ext) {
        Vector3 p;
        if (!m_point_ext->projectToPoint(state_id, p)) {
            return 0;
        }

        auto& goal_pose = planningSpace()->goal().pose;
        Vector3 gp(goal_pose.translation());

        double dist = computeDistance(p, gp);

        const int h = FIXED_POINT_RATIO * dist;
        SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
        return h;
    } else {
        return 0;
    }
}
示例#3
0
void cnoid::savePCD(SgPointSet* pointSet, const std::string& filename, const Affine3& viewpoint)
{
    if(!pointSet->hasVertices()){
        throw empty_data_error() << error_info_message(_("Empty pointset"));
    }

    ofstream ofs;
    ofs.open(filename.c_str());

    ofs <<
        "# .PCD v.7 - Point Cloud Data file format\n"
        "VERSION .7\n"
        "FIELDS x y z\n"
        "SIZE 4 4 4\n"
        "TYPE F F F\n"
        "COUNT 1 1 1\n";

    const SgVertexArray& points = *pointSet->vertices();
    const int numPoints = points.size();
    ofs << "WIDTH " << numPoints << "\n";
    ofs << "HEIGHT 1\n";

    ofs << "VIEWPOINT ";
    Affine3::ConstTranslationPart t = viewpoint.translation();
    ofs << t.x() << " " << t.y() << " " << t.z() << " ";
    const Quaternion q(viewpoint.rotation());
    ofs << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << "\n";

    ofs << "POINTS " << numPoints << "\n";
    
    ofs << "DATA ascii\n";

    for(int i=0; i < numPoints; ++i){
        const Vector3f& p = points[i];
        ofs << p.x() << " " << p.y() << " " << p.z() << "\n";
    }

    ofs.close();
}