Permalink
Browse files

adding files

  • Loading branch information...
1 parent c067ad0 commit eea09f7fc6e1dad444078bf5f9243ef106fd6447 Mike Krainin committed Jul 27, 2011
Showing with 265 additions and 0 deletions.
  1. +24 −0 CMakeLists.txt
  2. +36 −0 rosbuild_lite.cmake
  3. +37 −0 scripts/correct_test.py
  4. +11 −0 src/CMakeLists.txt
  5. +81 −0 src/Corrector.cpp
  6. +72 −0 src/DetectionSimulator.cpp
  7. +4 −0 src/ecto_corrector.cpp
View
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 2.8)
+project(ecto_corrector)
+message(STATUS "~~ ${PROJECT_NAME}")
+
+find_package(ecto REQUIRED)
+#find_package(Boost COMPONENTS signals thread REQUIRED)
+
+include(rosbuild_lite.cmake)
+find_ros_package(pose_corrector)
+if(NOT pose_corrector_FOUND)
+ message("**\n** Disabling build of ${PROJECT_NAME} due to missing dependency pose_corrector\n**")
+ return()
+endif()
+find_ros_package(sensor_msgs)
+find_ros_package(geometry_msgs)
+
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
+ecto_python_env_gen(${CMAKE_BINARY_DIR}/lib)
+
+include_directories(${sensor_msgs_INCLUDES})
+include_directories(${geometry_msgs_INCLUDES})
+include_directories(${pose_corrector_INCLUDES})
+
+add_subdirectory(src)
View
@@ -0,0 +1,36 @@
+macro (rospack VAR)
+ if(NOT ${VAR}_CACHED)
+ execute_process(COMMAND /usr/bin/env rospack ${ARGN}
+ OUTPUT_VARIABLE ${VAR}
+ ERROR_VARIABLE rospack_error
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+ if(rospack_error)
+ message(STATUS "Is your path setup correctly for ROS?\nrospack failed:${rospack_error}")
+ else()
+ unset(${VAR}_NOTFOUND)
+ separate_arguments(${VAR} UNIX_COMMAND ${${VAR}})
+ set(${VAR} ${${VAR}} CACHE STRING "${VAR} value")
+ set(${VAR}_CACHED TRUE CACHE BOOL "${VAR} cached flag")
+ #message(STATUS "Found ${VAR}.")
+ endif()
+ else()
+ #message(STATUS "BANG! Using cached ${VAR}")
+ endif()
+endmacro()
+
+macro (find_ros_package NAME)
+ rospack(${NAME}_INCLUDES cflags-only-I ${NAME})
+ if(rospack_error)
+ else()
+ set(${NAME}_FOUND TRUE)
+ include_directories(${${NAME}_INCLUDES})
+ rospack(${NAME}_DEFINITIONS cflags-only-other ${NAME})
+ foreach(DEF ${${NAME}_DEFINITIONS})
+ add_definitions(" ${DEF}")
+ endforeach()
+ rospack(${NAME}_LIBRARY_DIRS libs-only-L ${NAME})
+ rospack(${NAME}_LIBRARIES libs-only-l ${NAME})
+ link_directories(${${NAME}_LIBRARY_DIRS})
+ endif()
+endmacro()
View
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+
+import ecto
+import ecto_ros, ecto_sensor_msgs
+import ecto_corrector
+
+import sys
+
+ImageSub = ecto_sensor_msgs.Subscriber_Image
+Cloud2Sub = ecto_sensor_msgs.Subscriber_PointCloud2
+InfoSub = ecto_sensor_msgs.Subscriber_CameraInfo
+
+if __name__ == "__main__":
+ ecto_ros.init(sys.argv, "corrector_test")
+
+ ply_file = "/u/mkrainin/object_data/coke_can_scaled.ply"
+ fake_detection = ecto_corrector.DetectionSimulator("Fake Detection",
+ ply=ply_file, x=0, y=0.05, z=0.78, qx=0.91, qy=-0.01, qz=-0.02, qw=0.42)
+ sub_rgb = ImageSub("image sub",topic_name="/camera/rgb/image_color")
+ sub_cloud = Cloud2Sub("cloud sub",topic_name="/camera/rgb/points")
+ sub_info = InfoSub("info sub",topic_name="/camera/rgb/camera_info")
+ corrector = ecto_corrector.Corrector("Corrector")
+
+ graph = [
+ sub_rgb[:] >> corrector["image_color"],
+ sub_cloud[:] >> corrector["cloud"],
+ sub_info[:] >> corrector["camera_info"],
+ fake_detection["ply_file","output_pose"] >> corrector["ply_file","input_pose"]
+ ]
+
+ plasm = ecto.Plasm()
+ plasm.connect(graph)
+
+ ecto.view_plasm(plasm)
+
+ sched = ecto.schedulers.Threadpool(plasm)
+ sched.execute()
View
@@ -0,0 +1,11 @@
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+ectomodule(ecto_corrector
+ ecto_corrector.cpp
+ Corrector.cpp
+ DetectionSimulator.cpp)
+link_ecto(ecto_corrector
+ ${sensor_msgs_LIBRARIES}
+ ${geometry_msgs_LIBRARIES}
+ ${pose_corrector_LIBRARIES}
+ )
View
@@ -0,0 +1,81 @@
+/*
+ * Corrector.cpp
+ *
+ * Created on: Jul 19, 2011
+ * Author: mkrainin
+ */
+
+#include "ecto/ecto.hpp"
+
+#include "geometry_msgs/PoseStamped.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/CameraInfo.h"
+
+#include "pose_corrector/corrector.h"
+#include "pose_corrector/model/rigidObjectModel.h"
+
+namespace ecto_corrector
+{
+ using ecto::tendrils;
+
+ struct Corrector
+ {
+ public:
+ static void declare_params(tendrils& params)
+ {
+ //TODO: iterations, refine, etc
+ //e.g. params.declare<std::string>("prefix", "A prefix for printing");
+ }
+
+ static void declare_io(const tendrils& /*params*/, tendrils& in, tendrils& out)
+ {
+ in.declare<geometry_msgs::PoseStamped>(
+ "input_pose", "Initial object pose estimate", geometry_msgs::PoseStamped());
+ in.declare<std::string>(
+ "ply_file", "PLY file w/ triangulated mesh", "");
+ in.declare<sensor_msgs::CameraInfoConstPtr>(
+ "camera_info", "Camera info for rgb camera", sensor_msgs::CameraInfoConstPtr());
+ in.declare<sensor_msgs::ImageConstPtr>(
+ "image_color","Image for current sensor frame",sensor_msgs::ImageConstPtr());
+ in.declare<sensor_msgs::PointCloud2ConstPtr>(
+ "cloud","Point Cloud for current sensor frame",sensor_msgs::PointCloud2ConstPtr());
+ out.declare<geometry_msgs::PoseStamped>(
+ "output_pose", "Refined object pose estimate");
+ }
+
+ void configure(tendrils& params, tendrils& /*in*/, tendrils& /*out*/)
+ {
+ //TODO: copy out params/update structures
+ }
+
+ int process(const tendrils& in, tendrils& out)
+ {
+ tf::Transform init_pose;
+ tf::poseMsgToTF(in.get<geometry_msgs::PoseStamped>("input_pose").pose,init_pose);
+
+ //TODO: some of these structures should persist
+ pose_corrector::Corrector corrector;
+ boost::shared_ptr<pose_corrector::BaseModel> model(
+ new pose_corrector::RigidObjectModel(in.get<std::string>("ply_file")));
+ corrector.setModel(model);
+ corrector.setModelBasePose(init_pose);
+ corrector.initCamera(in.get<sensor_msgs::CameraInfoConstPtr>("camera_info"));
+
+ corrector.correct(in.get<sensor_msgs::ImageConstPtr>("image_color"),
+ in.get<sensor_msgs::PointCloud2ConstPtr>("cloud"));
+
+ geometry_msgs::PoseStamped final_pose;
+ final_pose.header = in.get<geometry_msgs::PoseStamped>("input_pose").header;
+ tf::poseTFToMsg(corrector.getModelBasePose(),final_pose.pose);
+
+ out.get<geometry_msgs::PoseStamped>("output_pose") = final_pose;
+
+ return ecto::OK;
+ }
+
+ private:
+ pose_corrector::OptimizerG2OParams opt_params;
+ };
+} //namespace
+ECTO_CELL(ecto_corrector, ecto_corrector::Corrector, "Corrector", "Performs pose refinement for rigid objects");
View
@@ -0,0 +1,72 @@
+/*
+ * DetectionSimulator.cpp
+ *
+ * Created on: Jul 26, 2011
+ * Author: mkrainin
+ */
+
+#include "ecto/ecto.hpp"
+#include "geometry_msgs/PoseStamped.h"
+
+namespace ecto_corrector
+{
+ using ecto::tendrils;
+
+ struct DetectionSimulator
+ {
+ public:
+ static void declare_params(tendrils& params)
+ {
+ params.declare<std::string>("ply", "PLY file w/ triangulated mesh");
+ params.declare<double>("x", "x component of the pose of the object wrt camera");
+ params.declare<double>("y", "y component of the pose of the object wrt camera");
+ params.declare<double>("z", "z component of the pose of the object wrt camera");
+ params.declare<double>("qx", "qx component of the pose of the object wrt camera");
+ params.declare<double>("qy", "qy component of the pose of the object wrt camera");
+ params.declare<double>("qz", "qz component of the pose of the object wrt camera");
+ params.declare<double>("qw", "qw component of the pose of the object wrt camera");
+ }
+
+ static void declare_io(const tendrils& /*params*/, tendrils& in, tendrils& out)
+ {
+ out.declare<std::string>("ply_file", "PLY file w/ triangulated mesh");
+ out.declare<geometry_msgs::PoseStamped>("output_pose", "Pose estimate");
+ }
+
+ void configure(tendrils& params, tendrils& /*in*/, tendrils& /*out*/)
+ {
+ ply=params.at("ply");
+ x=params.at("x");
+ y=params.at("y");
+ z=params.at("z");
+ qx=params.at("qx");
+ qy=params.at("qy");
+ qz=params.at("qz");
+ qw=params.at("qw");
+ }
+
+ int process(const tendrils& in, tendrils& out)
+ {
+ geometry_msgs::PoseStamped pose;
+ pose.header.stamp = ros::Time::now();
+ pose.header.frame_id = "/object";
+ pose.pose.position.x = x.read();
+ pose.pose.position.y = y.read();
+ pose.pose.position.z = z.read();
+ pose.pose.orientation.x = qx.read();
+ pose.pose.orientation.y = qy.read();
+ pose.pose.orientation.z = qz.read();
+ pose.pose.orientation.w = qw.read();
+
+ out.get<std::string>("ply_file") = ply.read();
+ out.get<geometry_msgs::PoseStamped>("output_pose") = pose;
+
+ return ecto::OK;
+ }
+
+ private:
+ ecto::spore<std::string> ply;
+ ecto::spore<double> x,y,z,qx,qy,qz,qw;
+ };
+} //namespace
+ECTO_CELL(ecto_corrector, ecto_corrector::DetectionSimulator, "DetectionSimulator", "Outputs ply and pose that were set manually");
View
@@ -0,0 +1,4 @@
+#include <ecto/ecto.hpp>
+
+//set up the module
+ECTO_DEFINE_MODULE(ecto_corrector) {}

0 comments on commit eea09f7

Please sign in to comment.