// recursive function to walk through tree
bool addChildrenToTree(urdf::LinkPtr root, Tree& tree)
{
urdf::LinkVector children = root->child_links;
//std::cerr << "[INFO] Link " << root->name << " had " << children.size() << " children" << std::endl;
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
std::cout<<"Link "<<root->name.c_str()<<" had "<<(int)children.size()<<" children."<<std::endl;
// ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
void SGMCensus::init(MultiViewMatcher& mvm, const cv::Mat& leftImg, const cv::Mat& rightImg)
{
height = leftImg.rows;
width = leftImg.cols;
dispCnt = params.dispEnd - params.dispStart + 1;
FeatureDescriptor::init(mvm, leftImg, rightImg);
census.init(mvm, leftImg, rightImg);
memset(data, 0xF, g_maxWidth*g_maxHeight*g_maxDispCnt*sizeof(int));
for(int row = 0; row < height; ++row)
for(int col = 0; col < width; ++col)
for(int disp = 0; disp < dispCnt; ++disp)
{
int cost = census(row, col, disp);
data[row][col][disp]= cost;
}
sgm();
}
void PathFinderManager::filterPastPoints(Vector<WorldCoordinates>* path, SceneObject* object) {
Vector3 thisWorldPosition = object->getWorldPosition();
Vector3 thiswP = thisWorldPosition;
thiswP.setZ(0);
int i = 2;
while (i < path->size()) {
WorldCoordinates coord1 = path->get(i);
WorldCoordinates coord2 = path->get(i - 1);
if (path->size() > 2) {
if (coord1 == coord2) {
path->remove(i - 1);
continue;
}
Vector3 end = coord1.getWorldPosition();
Vector3 start = coord2.getWorldPosition();
if (end == start) {
path->remove(i - 1);
continue;
}
end.setZ(0);
start.setZ(0);
Segment sgm(start, end);
Vector3 closestP = sgm.getClosestPointTo(thiswP);
if (closestP.distanceTo(thiswP) <= FLT_EPSILON) {
for (int j = i - 1; j > 0; --j) {
path->remove(j);
}
continue;
}
}
i++;
}
}
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
if (consider_root_link_inertia) {
//For giving a name to the root of KDL using the robot name,
//as it is not used elsewhere in the KDL tree
std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";
tree = Tree(fake_root_name);
const urdf::ConstLinkPtr root = robot_model.getRoot();
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);
// construct the kdl segment
Segment sgm(root->name, jnt, Frame::Identity(), inert);
// add segment to tree
tree.addSegment(sgm, fake_root_name);
} else {
tree = Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial)
std::cerr << "The root link " << robot_model.getRoot()->name <<
" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl;
}
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
void PathFinderManager::filterPastPoints(Vector<WorldCoordinates>* path, SceneObject* object) {
Vector3 thisWorldPosition = object->getWorldPosition();
Vector3 thiswP = thisWorldPosition;
thiswP.setZ(0);
if (path->size() > 2 && path->get(0) == path->get(1))
path->remove(1);
for (int i = 2; i < path->size(); ++i) {
WorldCoordinates coord1 = path->get(i);
WorldCoordinates coord2 = path->get(i - 1);
Vector3 end = coord1.getWorldPosition();
Vector3 start = coord2.getWorldPosition();
if (coord1.getCell() != coord2.getCell()) {
Vector3 coord1WorldPosition = end;
Vector3 coord2WorldPosition = start;
if (coord1WorldPosition == coord2WorldPosition && thisWorldPosition == coord1WorldPosition) {
path->remove(i - 1);
break;
}
continue;
}
end.setZ(0);
start.setZ(0);
Segment sgm(start, end);
Vector3 closestP = sgm.getClosestPointTo(thiswP);
if (closestP.distanceTo(thiswP) <= FLT_EPSILON) {
for (int j = i - 1; j > 0; --j) {
path->remove(j);
}
break;
}
}
}
int main(int argc, char* argv[]) {
const int disp_size = 128;
sl::Camera zed;
sl::InitParameters initParameters;
initParameters.camera_resolution = sl::RESOLUTION_VGA;
sl::ERROR_CODE err = zed.open(initParameters);
if (err != sl::SUCCESS) {
std::cout << toString(err) << std::endl;
zed.close();
return 1;
}
const int width = static_cast<int>(zed.getResolution().width);
const int height = static_cast<int>(zed.getResolution().height);
sl::Mat d_zed_image_l(zed.getResolution(), sl::MAT_TYPE_8U_C1, sl::MEM_GPU);
sl::Mat d_zed_image_r(zed.getResolution(), sl::MAT_TYPE_8U_C1, sl::MEM_GPU);
const int input_depth = 8;
const int input_bytes = input_depth * width * height / 8;
const int output_depth = 8;
const int output_bytes = output_depth * width * height / 8;
sgm::StereoSGM sgm(width, height, disp_size, input_depth, output_depth, sgm::EXECUTE_INOUT_CUDA2CUDA);
cv::Mat disparity(height, width, CV_8U);
cv::Mat disparity_8u, disparity_color;
device_buffer d_image_l(input_bytes), d_image_r(input_bytes), d_disparity(output_bytes);
while (1) {
if (zed.grab() == sl::SUCCESS) {
zed.retrieveImage(d_zed_image_l, sl::VIEW_LEFT_GRAY, sl::MEM_GPU);
zed.retrieveImage(d_zed_image_r, sl::VIEW_RIGHT_GRAY, sl::MEM_GPU);
} else continue;
cudaMemcpy2D(d_image_l.data, width, d_zed_image_l.getPtr<uchar>(sl::MEM_GPU), d_zed_image_l.getStep(sl::MEM_GPU), width, height, cudaMemcpyDeviceToDevice);
cudaMemcpy2D(d_image_r.data, width, d_zed_image_r.getPtr<uchar>(sl::MEM_GPU), d_zed_image_r.getStep(sl::MEM_GPU), width, height, cudaMemcpyDeviceToDevice);
const auto t1 = std::chrono::system_clock::now();
sgm.execute(d_image_l.data, d_image_r.data, d_disparity.data);
cudaDeviceSynchronize();
const auto t2 = std::chrono::system_clock::now();
const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
const double fps = 1e6 / duration;
cudaMemcpy(disparity.data, d_disparity.data, output_bytes, cudaMemcpyDeviceToHost);
disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size);
cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_JET);
cv::putText(disparity_color, format_string("sgm execution time: %4.1f[msec] %4.1f[FPS]", 1e-3 * duration, fps),
cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
cv::imshow("disparity", disparity_color);
const char c = cv::waitKey(1);
if (c == 27) // ESC
break;
}
zed.close();
return 0;
}
