bool yarp::dev::FrameTransformClient::transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) { if (input_point.size() != 3) { yError() << "sorry.. only 3 dimensional vector allowed my dear.."; return false; } yarp::sig::Matrix m(4, 4); if (!getTransform(target_frame_id, source_frame_id, m)) { yError() << "no transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'"; return false; } yarp::sig::Vector in = input_point; in.push_back(1); transformed_point = m * in; transformed_point.pop_back(); return true; }