Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
  • 4 commits
  • 25 files changed
  • 0 commit comments
  • 1 contributor
View
4 src/CMakeLists.txt
@@ -12,8 +12,8 @@ include_directories(
${LOG4CPLUS_INCLUDE_DIRS}
)
link_directories(${BULLETSIM_BINARY_DIR}/src)
-option(JOHNS_ADVENTURES OFF)
-option(JONATHANS_ADVENTURES OFF)
+option(JOHNS_ADVENTURES ON)
+option(JONATHANS_ADVENTURES ON)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
View
23 src/clouds/cloud_ops.cpp
@@ -218,7 +218,6 @@ ColorCloudPtr maskCloud(const ColorCloudPtr in, const cv::Mat& mask) {
assert(mask.elemSize() == 1);
assert(mask.rows == in->height);
assert(mask.cols == in->width);
- assert(in->isOrganized());
boost::shared_ptr< vector<int> > indicesPtr(new vector<int>());
@@ -262,3 +261,25 @@ void labelCloud(ColorCloudPtr in, const cv::Mat& labels) {
for (int i=0; i < in->size(); i++)
in->points[i]._unused = labels.at<uint8_t>(uv(i,0), uv(i,1));
}
+
+#include <opencv2/highgui/highgui.hpp>
+ColorCloudPtr hueFilter(const ColorCloudPtr in, uint8_t minHue, uint8_t maxHue, uint8_t minSat) {
+ MatrixXu bgr = toBGR(in);
+ int nPts = in->size();
+ cv::Mat cvmat(in->height,in->width, CV_8UC3, bgr.data());
+ cv::cvtColor(cvmat, cvmat, CV_BGR2HSV);
+ vector<cv::Mat> hsvChannels;
+ cv::split(cvmat, hsvChannels);
+
+ cv::Mat& h = hsvChannels[0];
+ cv::Mat& s = hsvChannels[1];
+ cv::Mat& l = hsvChannels[2];
+ cv::Mat hueMask = (minHue < maxHue) ?
+ (h > minHue) & (h < maxHue) :
+ (h > minHue) | (h < maxHue);
+ cv::Mat satMask = (s > minSat);
+ cv::Mat lumMask = (l > 100);
+ cv::Mat mask = hueMask & satMask;
+ //cv::imwrite("/tmp/blah.bmp", mask);
+ return maskCloud(in, hueMask & satMask & lumMask);
+}
View
2  src/clouds/cloud_ops.h
@@ -23,4 +23,4 @@ ColorCloudPtr maskCloud(const ColorCloudPtr in, const cv::Mat& mask);
ColorCloudPtr maskCloud(const ColorCloudPtr in, const VectorXb& mask);
ColorCloudPtr removeZRange(const ColorCloudPtr in, float minZ, float maxZ);
void labelCloud(ColorCloudPtr in, const cv::Mat& labels);
-
+ColorCloudPtr hueFilter(ColorCloudPtr in, uint8_t minHue, uint8_t maxHue, uint8_t minSat);
View
3  src/clouds/comm_rope_preproc.cpp
@@ -31,11 +31,10 @@ int main(int argc, char* argv[]) {
FileSubscriber cloudSub("kinect","pcd");
FilePublisher cloudPub("rope_pts","pcd");
- RopePreprocessor tp;
+ RopePreprocessor2 tp;
while (true) {
bool gotOne = cloudSub.recv(inputCloudMsg);
- cout << "GOTIT" << gotOne << endl;
if (!gotOne) break;
ColorCloudPtr outputCloud = tp.extractRopePoints(inputCloudMsg.m_data);
View
23 src/clouds/preprocessing.cpp
@@ -65,7 +65,6 @@ VectorXb getPointsOnTable(ColorCloudPtr cloudCam, const Affine3f& camToWorld, co
* (ptsWorld.col(2) >= minZ)
* (ptsWorld.col(2) <= maxZ);
- cout << out.sum() << endl;
return out;
}
@@ -145,5 +144,27 @@ ColorCloudPtr RopePreprocessor::extractRopePoints(ColorCloudPtr cloud) {
ColorCloudPtr downedTowelCloud = removeOutliers(downsampleCloud(towelCloud,.02),1.5);
return downedTowelCloud;
}
+
+RopePreprocessor2::RopePreprocessor2() {
+
+ vector<Vector3f> cornersCam = toEigenVectors(floatMatFromFile(onceFile("table_corners.txt").string()));
+ m_camToWorld = getCamToWorldFromTable(cornersCam);
+
+ MatrixXf cornersCam1(4,3);
+ for (int i=0; i < 4; i++) cornersCam1.row(i) = cornersCam[i];
+
+ Matrix3f rotation; rotation = m_camToWorld.linear();
+ m_cornersWorld = cornersCam1 * rotation.transpose();
+}
+
+
+ColorCloudPtr RopePreprocessor2::extractRopePoints(ColorCloudPtr cloud) {
+ VectorXb mask = getPointsOnTable(cloud, m_camToWorld, m_cornersWorld,.01,1);
+ ColorCloudPtr cloudOnTable = maskCloud(cloud, mask);
+
+ ColorCloudPtr redCloud = hueFilter(cloudOnTable, 170, 10, 220);
+ ColorCloudPtr downedRopeCloud = downsampleCloud(removeOutliers(redCloud,1),.01);
+ return downedRopeCloud;
+}
View
12 src/clouds/preprocessing.h
@@ -36,6 +36,18 @@ class RopePreprocessor {
};
+class RopePreprocessor2 {
+public:
+
+ Eigen::MatrixXf m_cornersWorld;
+ Eigen::Affine3f m_camToWorld;
+
+ RopePreprocessor2();
+ ColorCloudPtr extractRopePoints(ColorCloudPtr cloud);
+
+};
+
+
/*
class TowelGrabAndProc : public CloudGrabber {
View
2  src/clouds/utils_pcl.h
@@ -13,7 +13,7 @@ typedef pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ColorCloudPtr;
typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
typedef Eigen::Matrix<uint8_t,Eigen::Dynamic,Eigen::Dynamic> MatrixXu;
-pcl::PointCloud<pcl::PointXYZRGBA>::Ptr readPCD(const std::string& pcdfile);
+ColorCloudPtr readPCD(const std::string& pcdfile);
Eigen::MatrixXi xyz2uv(const Eigen::MatrixXf& xyz);
View
6 src/comm/CMakeLists.txt
@@ -2,7 +2,7 @@ add_library(comm SHARED comm.cpp comm_eigen.cpp)
target_link_libraries(comm ${Boost_LIBRARIES} json)
add_executable(test_comm test_comm.cpp)
-target_link_libraries(test_comm comm ${Boost_LIBRARIES} json)
+target_link_libraries(test_comm comm ${Boost_LIBRARIES} json utils)
-INSTALL(TARGETS comm DESTINATION "~/usr/lib")
-SET(CMAKE_INSTALL_RPATH "~/usr/lib")
+#INSTALL(TARGETS comm DESTINATION "~/usr/lib")
+#SET(CMAKE_INSTALL_RPATH "~/usr/lib")
View
3  src/comm/comm.cpp
@@ -8,6 +8,7 @@
#include <unistd.h>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
+#include "utils/logging.h"
using namespace std;
path DATA_ROOT = "/dont/forget/to/set";
@@ -247,7 +248,7 @@ bool FileSubscriber::recv(Message& message, bool enableWait) {
m_names.step();
}
if (!gotIt) {
- cout << "didn't get " << namePair.second << endl;
+ LOG_DEBUG("didn't get " << namePair.second);
}
return gotIt;
}
View
15 src/john_sandbox/CMakeLists.txt
@@ -58,4 +58,17 @@ add_executable(test_rave_load test_rave_load.cpp)
target_link_libraries(test_rave_load simulation robots)
add_executable(test_dynamic_pr2 test_dynamic_pr2.cpp)
-target_link_libraries(test_dynamic_pr2 simulation robots)
+target_link_libraries(test_dynamic_pr2 simulation robots)
+
+find_package(catkin REQUIRED)
+set(hydra_msgs_DIR "~/Dropbox/myros/hydra_msgs/build/cmake/hydra_msgs")
+find_package(ROS REQUIRED COMPONENTS
+ cpp_common rostime roscpp_traits
+ roscpp_serialization sensor_msgs geometry_msgs hydra_msgs
+ roscpp rosconsole
+)
+include_directories(${ROS_INCLUDE_DIRS})
+include_directories("/home/joschu/Dropbox/myros/hydra_msgs/msg_gen/cpp/include")
+message("${ROS_LIBRARIES}")
+add_executable(test_ros test_ros.cpp)
+target_link_libraries(test_ros ${ROS_LIBRARIES})
View
22 src/john_sandbox/test_ros.cpp
@@ -0,0 +1,22 @@
+#include "sensor_msgs/Image.h"
+#include "hydra_msgs/Calib.h"
+#include <iostream>
+#include <ros/ros.h>
+using namespace std;
+
+
+void callback(const hydra_msgs::Calib& msg) {
+ cout << msg << endl;
+}
+
+int main(int argc, char* argv[]) {
+ sensor_msgs::Image p;
+ cout << p << endl;
+
+ ros::init(argc, argv, "test_recv_hydra_msg");
+ ros::NodeHandle nh;
+
+ ros::Subscriber sub = nh.subscribe("hydra_calib", 1000, callback);
+ ros::spin();
+ return 0;
+}
View
2  src/perception/config_perception.h
@@ -32,7 +32,7 @@ struct TrackingConfig : Config {
params.push_back(new Parameter<float>("sigB", &sigB, "variance on observed positions"));
params.push_back(new Parameter<float>("impulseSize", &impulseSize, "scaling from log-lik gradient to impulse"));
params.push_back(new Parameter<float>("cutoff", &cutoff, "smallest correspondence value"));
- params.push_back(new Parameter<float>("outlierParam", &cutoff, "p(outlier) * density"));
+ params.push_back(new Parameter<float>("outlierParam", &outlierParam, "p(outlier) * density"));
params.push_back(new Parameter<int>("nSamples", &nSamples, "smallest correspondence value"));
params.push_back(new Parameter<int>("stepsPerM",&stepsPerM,"number of physics steps in M step of algorithm"));
params.push_back(new Parameter<float>("kp",&kp,"kp"));
View
20 src/perception/rope_tracking.cpp
@@ -12,8 +12,8 @@ using namespace Eigen;
#include "perception/make_bodies.h"
#include "utils/vector_io.h"
#include "perception/apply_impulses.h"
-
-static const float LABEL_MULTIPLIER = 100; //just so things work with old rope_pts data
+#include "utils/logging.h"
+static const float LABEL_MULTIPLIER = 0; //just so things work with old rope_pts data
static osg::Vec4f hyp_colors[6] = {osg::Vec4f(1,0,0,.3),
@@ -98,6 +98,7 @@ void TrackedRope::applyEvidence(const SparseArray& corr, const MatrixXf& obsPts)
vector<btVector3> ropePos = getNodes(m_sim);
vector<btVector3> ropeVel = getNodeVels(m_sim);
vector<btVector3> impulses = calcImpulsesDamped(ropePos, ropeVel, toBulletVectors(obsPts), corr, masses, TrackingConfig::kp, TrackingConfig::kd);
+ //m_sigs = 1*VectorXf::Ones(ropePos.size());
m_sigs = calcSigs(corr, toEigenMatrix(ropePos), obsPts, .1*METERS, 1);
applyImpulses(impulses, m_sim);
}
@@ -170,11 +171,10 @@ void SingleHypRopeTracker::doIteration() {
VectorXf pVis = calcVisibility(m_hyp->m_tracked->m_sim->children, m_scene->env->bullet->dynamicsWorld, m_CT->worldFromCamUnscaled.getOrigin()*METERS, TrackingConfig::sigA*METERS, TrackingConfig::nSamples);
Eigen::MatrixXf corrEigen = calcCorrProb(tr->featsFromSim(), tr->m_sigs, m_obsFeats, pVis, TrackingConfig::outlierParam, m_hyp->m_loglik);
+ //cout << corrEigen << endl;
+ cout << "cut " << TrackingConfig::cutoff << endl;
SparseArray corr = toSparseArray(corrEigen, TrackingConfig::cutoff);
tr->applyEvidence(corr, toEigenMatrix(m_obsCloud));
-
-
-
vector<btVector3> obsPts = toBulletVectors(m_obsCloud);
vector<btVector3> estPts = getNodes(tr->m_sim);
Eigen::VectorXf inlierFrac = corrEigen.colwise().sum();
@@ -192,7 +192,7 @@ void SingleHypRopeTracker::afterIterations() {
}
DefaultSingleHypRopeTracker::DefaultSingleHypRopeTracker() : SingleHypRopeTracker() {
- m_multisub = new RopeSubs2();
+ m_multisub = new RopeSubs();
Tracker2::m_multisub = m_multisub;
SingleHypRopeTracker::m_multisub = m_multisub;
@@ -206,9 +206,9 @@ DefaultSingleHypRopeTracker::DefaultSingleHypRopeTracker() : SingleHypRopeTracke
void DefaultSingleHypRopeTracker::beforeIterations() {
SingleHypRopeTracker::beforeIterations();
ColorCloudPtr cloudCam = m_multisub->m_kinectMsg.m_data;
- cv::Mat labels = m_multisub->m_labelMsg.m_data;
- m_ropeMask = labels < 2;
- m_depthImage = getDepthImage(cloudCam);
+ //cv::Mat labels = m_multisub->m_labelMsg.m_data;
+ //m_ropeMask = labels < 2;
+ //m_depthImage = getDepthImage(cloudCam);
}
@@ -446,7 +446,7 @@ void MultiHypRobotAndRopeTracker::beforeIterations() {
MultiHypRopeTracker::beforeIterations();
-}
+p}
void MultiHypRobotAndRopeTracker::afterIterations() {
MultiHypRopeTracker::afterIterations();
}
View
2  src/perception/rope_tracking.h
@@ -123,7 +123,7 @@ struct SingleHypRopeTracker : public Tracker2 {
};
struct DefaultSingleHypRopeTracker : public SingleHypRopeTracker {
- RopeSubs2* m_multisub;
+ RopeSubs* m_multisub;
DefaultSingleHypRopeTracker();
virtual void beforeIterations();
};
View
6 src/perception/tracking.cpp
@@ -93,6 +93,8 @@ void Tracker2::runOnline() {
}
void Tracker2::runOffline() {
+ m_scene->idle(true);
+ cout << "idling..." << endl;
for (int t=0; ; t++) {
cout << "t=" << t << endl;
int iter=0;
@@ -100,10 +102,10 @@ void Tracker2::runOffline() {
ENSURE(m_multisub->recvAll());
beforeIterations();
- cout << "iter: ";
do {
doIteration();
- cout << iter++ << " ";
+ cout << "iteration" << iter << endl;
+ iter++;
}
while (iter < TrackingConfig::nIter);
cout << endl;
View
1  src/simulation/CMakeLists.txt
@@ -20,7 +20,6 @@ add_library(simulation
config_viewer.cpp
bullet_io.cpp
fake_gripper.cpp
- logging.cpp
)
target_link_libraries(simulation
View
3  src/simulation/openravesupport.cpp
@@ -4,6 +4,7 @@
#include "convexdecomp.h"
#include "utils/config.h"
#include "bullet_io.h"
+#include "utils/logging.h"
using namespace OpenRAVE;
using namespace std;
@@ -21,7 +22,7 @@ RaveInstance::RaveInstance() {
isRoot = true;
RaveInitialize(true);
env = RaveCreateEnvironment();
- if (GeneralConfig::verbose)
+ if (GeneralConfig::verbose <= log4cplus::DEBUG_LOG_LEVEL)
RaveSetDebugLevel(Level_Debug);
}
View
2  src/simulation/plotting.cpp
@@ -147,7 +147,7 @@ PlotSpheres::PlotSpheres() {
osg::ref_ptr<osg::BlendFunc> blendFunc = new osg::BlendFunc;
blendFunc->setFunction(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
stateset->setAttributeAndModes(blendFunc);
- stateset->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+ stateset->setMode(GL_BLEND, osg::StateAttribute::ON);
m_geode->setStateSet(stateset);
View
2  src/tests/test_logging.cpp
@@ -1,4 +1,4 @@
-#include "simulation/logging.h"
+#include "utils/logging.h"
int main(int argc, char *argv[]) {
LoggingInit();
View
3  src/utils/CMakeLists.txt
@@ -1 +1,2 @@
-add_library(utils config.cpp vector_io.cpp yes_or_no.cpp interpolation.cpp)
+add_library(utils config.cpp vector_io.cpp yes_or_no.cpp interpolation.cpp logging.cpp)
+target_link_libraries(utils ${LOG4CPLUS_LIBRARY})
View
10 src/utils/config.cpp
@@ -1,4 +1,10 @@
#include "config.h"
+#include "utils/logging.h"
+#include <log4cplus/logger.h>
+
+
+
+
using namespace std;
void Parser::read(int argc, char* argv[]) {
@@ -22,8 +28,10 @@ param->addToBoost(od);
}
po::notify(vm);
+ LoggingInit();
+ log4cplus::Logger::getRoot().setLogLevel(GeneralConfig::verbose);
}
-bool GeneralConfig::verbose = false;
+int GeneralConfig::verbose = log4cplus::WARN_LOG_LEVEL;
float GeneralConfig::scale = 1.;
View
4 src/utils/config.h
@@ -43,10 +43,10 @@ class Parser {
};
struct GeneralConfig : Config {
- static bool verbose;
+ static int verbose;
static float scale;
GeneralConfig() : Config() {
- params.push_back(new Parameter<bool>("verbose", &verbose, "verbose switch"));
+ params.push_back(new Parameter<int>("verbose", &verbose, "verbosity: 0: debug, 10000:info, 20000: warn, 30000: error"));
params.push_back(new Parameter<float>("scale", &scale, "scale factor applied to distances that are assumed to be in meters"));
}
};
View
0  src/simulation/logging.cpp → src/utils/logging.cpp
File renamed without changes
View
18 src/utils/logging.h
@@ -1,8 +1,16 @@
-#ifndef __LOGGING_H__
-#define __LOGGING_H__
+#ifndef _LOGGING_H_
+#define _LOGGING_H_
-#include <boost/log/trivial.hpp>
+#include <log4cplus/logger.h>
+#include <log4cplus/loggingmacros.h>
-#define LOG BOOST_LOG_TRIVIAL
+#define LOG_TRACE(s) LOG4CPLUS_TRACE(log4cplus::Logger::getRoot(), s)
+#define LOG_DEBUG(s) LOG4CPLUS_DEBUG(log4cplus::Logger::getRoot(), s)
+#define LOG_INFO(s) LOG4CPLUS_INFO(log4cplus::Logger::getRoot(), s)
+#define LOG_WARN(s) LOG4CPLUS_WARN(log4cplus::Logger::getRoot(), s)
+#define LOG_ERROR(s) LOG4CPLUS_ERROR(log4cplus::Logger::getRoot(), s)
+#define LOG_FATAL(s) LOG4CPLUS_FATAL(log4cplus::Logger::getRoot(), s)
-#endif // __LOGGING_H__
+void LoggingInit();
+
+#endif // _LOGGING_H_
View
1  stack.yaml
@@ -0,0 +1 @@
+Depends: catkin, hydra_msgs

No commit comments for this range

Something went wrong with that request. Please try again.