void GestureClassifierByHistogram::drawMatchedIdPatternHistogram(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const std::string &windowName) const
{
// calculate matched index histogram
cv::MatND hist;
#if defined(__GNUC__)
{
cv::Mat tmpmat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end()));
cv::calcHist(&tmpmat, 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false);
}
#else
cv::calcHist(&cv::Mat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())), 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false);
#endif
// normalize histogram
//HistogramUtil::normalizeHistogram(hist, params_.maxMatchedHistogramNum);
// draw matched index histogram
cv::Mat histImg(cv::Mat::zeros(local::indexHistMaxHeight, local::indexHistBins*local::indexHistBinWidth, CV_8UC3));
HistogramUtil::drawHistogram1D(hist, local::indexHistBins, params_.maxMatchedHistogramNum, local::indexHistBinWidth, local::indexHistMaxHeight, histImg);
std::ostringstream sstream;
sstream << "count: " << matchedHistogramIndexes.size();
cv::putText(histImg, sstream.str(), cv::Point(10, 15), cv::FONT_HERSHEY_COMPLEX, 0.5, CV_RGB(255, 0, 255), 1, 8, false);
cv::imshow(windowName, histImg);
}
void ImageResizer::process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info,
sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info){
int image_width, image_height;
if(use_camera_info_) {
image_width = src_info->width;
image_height = src_info->height;
}
else {
image_width = src_img->width;
image_height = src_img->height;
}
int width = dst_width_ ? dst_width_ : (resize_x_ * image_width);
int height = dst_height_ ? dst_height_ : (resize_y_ * image_height);
double scale_x = dst_width_ ? ((double)dst_width_)/image_width : resize_x_;
double scale_y = dst_height_ ? ((double)dst_height_)/image_height : resize_y_;
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(src_img);
cv::Mat tmpmat(height, width, cv_img->image.type());
if (raw_width_ == 0) {
raw_width_ = tmpmat.cols;
raw_height_ = tmpmat.rows;
}
cv::resize(cv_img->image, tmpmat, cv::Size(width, height), 0, 0, interpolation_);
NODELET_DEBUG("mat rows:%d cols:%d", tmpmat.rows, tmpmat.cols);
cv_img->image = tmpmat;
dst_img = cv_img->toImageMsg();
if (use_camera_info_) {
dst_info = *src_info;
dst_info.height = height;
dst_info.width = width;
dst_info.K[0] = dst_info.K[0] * scale_x; // fx
dst_info.K[2] = dst_info.K[2] * scale_x; // cx
dst_info.K[4] = dst_info.K[4] * scale_y; // fy
dst_info.K[5] = dst_info.K[5] * scale_y; // cy
dst_info.P[0] = dst_info.P[0] * scale_x; // fx
dst_info.P[2] = dst_info.P[2] * scale_x; // cx
dst_info.P[3] = dst_info.P[3] * scale_x; // T
dst_info.P[5] = dst_info.P[5] * scale_y; // fy
dst_info.P[6] = dst_info.P[6] * scale_y; // cy
}
}
void callback(const sensor_msgs::ImageConstPtr &img,
const sensor_msgs::CameraInfoConstPtr &info) {
boost::mutex::scoped_lock lock(mutex_);
ros::Time now = ros::Time::now();
static boost::circular_buffer<double> in_times(100);
static boost::circular_buffer<double> out_times(100);
static boost::circular_buffer<double> in_bytes(100);
static boost::circular_buffer<double> out_bytes(100);
ROS_DEBUG("resize: callback");
if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
return;
}
in_times.push_front((now - last_subscribe_time_).toSec());
in_bytes.push_front(img->data.size());
//
try {
int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);
cv::Mat tmpmat(height, width, cv_img->image.type());
cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
cv_img->image = tmpmat;
sensor_msgs::CameraInfo tinfo = *info;
tinfo.height = height;
tinfo.width = width;
tinfo.K[0] = tinfo.K[0] * scale_x; // fx
tinfo.K[2] = tinfo.K[2] * scale_x; // cx
tinfo.K[4] = tinfo.K[4] * scale_y; // fy
tinfo.K[5] = tinfo.K[5] * scale_y; // cy
tinfo.P[0] = tinfo.P[0] * scale_x; // fx
tinfo.P[2] = tinfo.P[2] * scale_x; // cx
tinfo.P[3] = tinfo.P[3] * scale_x; // T
tinfo.P[5] = tinfo.P[5] * scale_y; // fy
tinfo.P[6] = tinfo.P[6] * scale_y; // cy
if ( !use_messages_ || now - last_publish_time_ > period_ ) {
cp_.publish(cv_img->toImageMsg(),
boost::make_shared<sensor_msgs::CameraInfo> (tinfo));
out_times.push_front((now - last_publish_time_).toSec());
out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());
last_publish_time_ = now;
}
} catch( cv::Exception& e ) {
ROS_ERROR("%s", e.what());
}
float duration = (now - last_rosinfo_time_).toSec();
if ( duration > 2 ) {
int in_time_n = in_times.size();
int out_time_n = out_times.size();
double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;
std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
in_time_mean /= in_time_n;
in_time_rate /= in_time_mean;
std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
if ( in_time_n > 1 ) {
in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
}
std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
out_time_mean /= out_time_n;
out_time_rate /= out_time_mean;
std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
if ( out_time_n > 1 ) {
out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
}
double in_byte_mean = 0, out_byte_mean = 0;
std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
in_byte_mean /= duration;
out_byte_mean /= duration;
ROS_INFO_STREAM(" in bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << in_byte_mean/1000*8
<< " Kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << out_byte_mean/1000*8
<< " kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
in_times.clear();
in_bytes.clear();
out_times.clear();
out_bytes.clear();
last_rosinfo_time_ = now;
}
last_subscribe_time_ = now;
if(use_snapshot_) {
publish_once_ = false;
}
}
void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
JSK_ROS_DEBUG("DepthImageCreator::publish_points");
if (!pcloud2) return;
bool proc_cloud = true, proc_image = true, proc_disp = true;
if ( pub_cloud_.getNumSubscribers()==0 ) {
proc_cloud = false;
}
if ( pub_image_.getNumSubscribers()==0 ) {
proc_image = false;
}
if ( pub_disp_image_.getNumSubscribers()==0 ) {
proc_disp = false;
}
if( !proc_cloud && !proc_image && !proc_disp) return;
int width = info->width;
int height = info->height;
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
Eigen::Affine3f sensorPose;
{
tf::StampedTransform transform;
if(use_fixed_transform) {
transform = fixed_transform;
} else {
try {
tf_listener_->waitForTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp,
ros::Duration(0.001));
tf_listener_->lookupTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp, transform);
}
catch ( std::runtime_error e ) {
JSK_ROS_ERROR("%s",e.what());
return;
}
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
if (tx != 0.0) {
Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
sensorPose = sensorPose * trans;
}
#if 0 // debug print
JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
}
PointCloud pointCloud;
pcl::RangeImagePlanar rangeImageP;
{
// code here is dirty, some bag is in RangeImagePlanar
PointCloud tpc;
pcl::fromROSMsg(*pcloud2, tpc);
Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
inv = sensorPose.inverse();
pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
pcl::getInverse(sensorPose, inv);
pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif
Eigen::Affine3f dummytrans;
dummytrans.setIdentity();
rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
width/scale_depth, height/scale_depth,
cx/scale_depth, cy/scale_depth,
fx/scale_depth, fy/scale_depth,
dummytrans); //sensorPose);
}
cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
float *tmpf = (float *)mat.ptr();
for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
tmpf[i] = rangeImageP.points[i].z;
}
if(scale_depth != 1.0) {
cv::Mat tmpmat(info->height, info->width, CV_32FC1);
cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
//cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
mat = tmpmat;
}
if (proc_image) {
sensor_msgs::Image pubimg;
pubimg.header = info->header;
pubimg.width = info->width;
pubimg.height = info->height;
pubimg.encoding = "32FC1";
pubimg.step = sizeof(float)*info->width;
pubimg.data.resize(sizeof(float)*info->width*info->height);
// publish image
memcpy(&(pubimg.data[0]), mat.ptr(), sizeof(float)*info->height*info->width);
pub_image_.publish(boost::make_shared<sensor_msgs::Image>(pubimg));
}
if(proc_cloud || proc_disp) {
// publish point cloud
pcl::RangeImagePlanar rangeImagePP;
rangeImagePP.setDepthImage ((float *)mat.ptr(),
width, height,
cx, cy, fx, fy);
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
rangeImagePP.header = pcl_conversions::toPCL(info->header);
#else
rangeImagePP.header = info->header;
#endif
if(proc_cloud) {
pub_cloud_.publish(boost::make_shared<pcl::PointCloud<pcl::PointWithRange > >
( (pcl::PointCloud<pcl::PointWithRange>)rangeImagePP) );
}
if(proc_disp) {
stereo_msgs::DisparityImage disp;
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
#else
disp.header = rangeImagePP.header;
#endif
disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp.image.height = rangeImagePP.height;
disp.image.width = rangeImagePP.width;
disp.image.step = disp.image.width * sizeof(float);
disp.f = fx; disp.T = 0.075;
disp.min_disparity = 0;
disp.max_disparity = disp.T * disp.f / 0.3;
disp.delta_d = 0.125;
disp.image.data.resize (disp.image.height * disp.image.step);
float *data = reinterpret_cast<float*> (&disp.image.data[0]);
float normalization_factor = disp.f * disp.T;
for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
data[y*disp.image.width+x] = normalization_factor / p.z;
}
}
pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
}
}
}
void WeightMatrix_iter::weight_matrix_parallel(std::vector< std::vector<double> >& allfeatures,
bool exhaustive)
{
printf("running parallel version\n");
std::vector< std::vector<double> > pfeatures;
copy(allfeatures, pfeatures);
_nrows = pfeatures.size();
_ncols = pfeatures[0].size();
EstimateBandwidth(pfeatures, _deltas);
_wtmat.resize(_nrows);
_degree.resize(_nrows, 0.0);
size_t NCORES = 8;
std::map<double, unsigned int> sorted_features; // all features are unique, check learn or iterative learn algo
std::vector< std::map<double, unsigned int> > sorted_features_p(NCORES); // all features are unique, check learn or iterative learn algo
int part = 0;
for(size_t i=0; i < pfeatures.size(); i++){
std::vector<double>& tmpvec = pfeatures[i];
scale_vec(tmpvec, _deltas);
double nrm = vec_norm(tmpvec);
sorted_features.insert(std::make_pair(nrm, i));
(sorted_features_p[part]).insert(std::make_pair(nrm, i));
part = (part+1) % NCORES;
}
std::vector< boost::thread* > threads;
std::vector< std::vector< std::vector<RowVal> > > tmpmat(sorted_features_p.size());
std::vector< std::vector<double> > degrees(sorted_features_p.size());
for(size_t pp=0; pp < sorted_features_p.size(); pp++){
tmpmat[pp].resize(_nrows);
degrees[pp].resize(_nrows, 0.0);
//compute_weight_partial(sorted_features_p[i], sorted_features, pfeatures);
threads.push_back(new boost::thread(&WeightMatrix_iter::compute_weight_partial, this, sorted_features_p[pp], sorted_features, pfeatures, boost::ref(tmpmat[pp]), boost::ref(degrees[pp])));
}
printf("Sync all threads \n");
for (size_t ti=0; ti<threads.size(); ti++)
(threads[ti])->join();
printf("all threads done\n");
for(size_t pp=0; pp < tmpmat.size(); pp++){
for(size_t i=0; i < tmpmat[pp].size(); i++){
if(tmpmat[pp][i].size()>0){
_wtmat[i].insert(_wtmat[i].end(),tmpmat[pp][i].begin(), tmpmat[pp][i].end());
(_degree[i]) += degrees[pp][i];
}
}
}
if(_nrows != _wtmat.size()){
printf(" number of rows and wtmat size mismatch\n");
}
_nzd_indicator.resize(_nrows, 0);
_trn_indicator.resize(_nrows, 0);
for (size_t i=0; i < _nrows; i++){
if (_degree[i] > 0){
_nzd_indicator[i] = 1;
_nzd_map.insert(std::make_pair(i, _nzd_count) );
_nzd_invmap.insert(std::make_pair(_nzd_count, i) );
_nzd_count++;
}
}
compute_Wnorm();
// //** _nzd_count = non-zero degree count == number of columns
// //** _nnz = number of nonzeros
//
// _nnz = _Wnorm_i.size();
// cholmod_solver.initialize(_nzd_count, _nnz, _Wnorm_i.data(), _Wnorm_j.data(), _Wnorm_v.data());
/* C debug*
FILE* fp=fopen("semi-supervised/tmp_wtmatrix_parallel.txt", "wt");
for(size_t rr = 0; rr < _wtmat.size(); rr++){
double luu = 0;
double w_l = 0;
for(size_t cc=0; cc < _wtmat[rr].size(); cc++){
size_t cidx = _wtmat[rr][cc].j;
double val = _wtmat[rr][cc].v;
//val = exp(-val/(0.5*_thd*_thd));
fprintf(fp, "%u %u %lf\n", rr, cidx, val);
}
}
fclose(fp);
//*C*/
}
