void ColorEdge::detectColorEdge(const cv::Mat_<cv::Vec3b> &image, cv::Mat_<uchar> &edge) { cv::Mat_<double> edge_map(image.size()); const int filter_half = static_cast<int>(filter_size_ / 2); for(int y = filter_half; y < (edge_map.rows - filter_half); ++y) { for(int x = filter_half; x < (edge_map.cols - filter_half); ++x) { cv::Mat_<cv::Vec3b> roi(image, cv::Rect(x - filter_half, y - filter_half, filter_size_, filter_size_)); edge_map(y, x) = calculateMVD(roi); } } edge_map.convertTo(edge, edge.type()); }
SeedFeature::SeedFeature( const ImageOverSegmentation & ios, const VectorXf & obj_param ) { Image rgb_im = ios.image(); const RMatrixXs & s = ios.s(); const int Ns = ios.Ns(), W = rgb_im.W(), H = rgb_im.H(); // Initialize various values VectorXf area = bin( s, 1, [&](int x, int y){ return 1.f; } ); VectorXf norm = (area.array()+1e-10).cwiseInverse(); pos_ = norm.asDiagonal() * bin( s, 6, [&](int i, int j){ float x=1.0*i/(W-1)-0.5,y=1.0*j/(H-1)-0.5; return makeArray<6>( x, y, x*x, y*y, fabs(x), fabs(y) ); } ); if (N_DYNAMIC_COL) { Image lab_im; rgb2lab( lab_im, rgb_im ); col_ = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( rgb_im(y,x, 0), rgb_im(y,x,1), rgb_im(y,x,2), lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2) ); } ); } const int N_GEO = sizeof(EDGE_P)/sizeof(EDGE_P[0]); for( int i=0; i<N_GEO; i++ ) gdist_.push_back( GeodesicDistance(ios.edges(),ios.edgeWeights().array().pow(EDGE_P[i])+1e-3) ); // Compute the static features static_f_ = RMatrixXf::Zero( Ns, N_STATIC_F ); int o=0; // Add the position features static_f_.block( 0, o, Ns, N_STATIC_POS ) = pos_.leftCols( N_STATIC_POS ); o += N_STATIC_POS; // Add the geodesic features if( N_STATIC_GEO >= N_GEO ) { RMatrixXu8 bnd = findBoundary( s ); RMatrixXf mask = (bnd.array() == 0).cast<float>()*1e10; for( int i=0; i<N_GEO; i++ ) static_f_.col( o++ ) = gdist_[i].compute( mask ); for( int j=1; (j+1)*N_GEO<=N_STATIC_GEO; j++ ) { mask = (bnd.array() != j).cast<float>()*1e10; for( int i=0; i<N_GEO; i++ ) static_f_.col( o++ ) = gdist_[i].compute( mask ); } } if( N_STATIC_EDGE ) { RMatrixXf edge_map = DirectedSobel().detect( ios.image() ); for( int j=0; j<s.rows(); j++ ) for( int i=0; i<s.cols(); i++ ) { const int id = s(j,i); int bin = edge_map(j,i)*N_STATIC_EDGE; if ( bin < 0 ) bin = 0; if ( bin >= N_STATIC_EDGE ) bin = N_STATIC_EDGE-1; static_f_(id,o+bin) += norm[id]; } o += N_STATIC_EDGE; } if( N_OBJ_F>1 ) static_f_.col(o++) = (computeObjFeatures(ios)*obj_param).transpose(); // Initialize the dynamic features dynamic_f_ = RMatrixXf::Zero( Ns, N_DYNAMIC_F ); n_ = 0; min_dist_ = RMatrixXf::Ones(Ns,5)*10; var_ = RMatrixXf::Zero(Ns,6); }
/*===========================================================================*/ void ExtractEdges::calculate_quadratic_tetrahedra_connections( const kvs::UnstructuredVolumeObject* volume ) { const kvs::UInt32* connections = volume->connections().data(); const size_t ncells = volume->numberOfCells(); const size_t nnodes = volume->numberOfNodes(); ::EdgeMap edge_map( nnodes ); for ( size_t cell_index = 0, connection_index = 0; cell_index < ncells; cell_index++ ) { const kvs::UInt32 local_vertex0 = connections[ connection_index ]; const kvs::UInt32 local_vertex1 = connections[ connection_index + 1 ]; const kvs::UInt32 local_vertex2 = connections[ connection_index + 2 ]; const kvs::UInt32 local_vertex3 = connections[ connection_index + 3 ]; const kvs::UInt32 local_vertex4 = connections[ connection_index + 4 ]; const kvs::UInt32 local_vertex5 = connections[ connection_index + 5 ]; const kvs::UInt32 local_vertex6 = connections[ connection_index + 6 ]; const kvs::UInt32 local_vertex7 = connections[ connection_index + 7 ]; const kvs::UInt32 local_vertex8 = connections[ connection_index + 8 ]; const kvs::UInt32 local_vertex9 = connections[ connection_index + 9 ]; connection_index += 10; edge_map.insert( local_vertex0, local_vertex4 ); edge_map.insert( local_vertex4, local_vertex1 ); edge_map.insert( local_vertex0, local_vertex5 ); edge_map.insert( local_vertex5, local_vertex2 ); edge_map.insert( local_vertex0, local_vertex6 ); edge_map.insert( local_vertex6, local_vertex3 ); edge_map.insert( local_vertex1, local_vertex7 ); edge_map.insert( local_vertex7, local_vertex2 ); edge_map.insert( local_vertex2, local_vertex8 ); edge_map.insert( local_vertex8, local_vertex3 ); edge_map.insert( local_vertex3, local_vertex9 ); edge_map.insert( local_vertex9, local_vertex1 ); } SuperClass::setConnections( edge_map.serialize() ); }
RMatrixXf SeedFeature::computeObjFeatures( const ImageOverSegmentation & ios ) { Image rgb_im = ios.image(); const RMatrixXs & s = ios.s(); const Edges & g = ios.edges(); const int Ns = ios.Ns(); RMatrixXf r = RMatrixXf::Zero( Ns, N_OBJ_F ); if( N_OBJ_F<=1 ) return r; VectorXf area = bin( s, 1, [&](int x, int y){ return 1.f; } ); VectorXf norm = (area.array()+1e-10).cwiseInverse(); r.col(0).setOnes(); int o = 1; if (N_OBJ_COL>=6) { Image lab_im; rgb2lab( lab_im, rgb_im ); r.middleCols(o,6) = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2), lab_im(y,x,0)*lab_im(y,x,0), lab_im(y,x,1)*lab_im(y,x,1), lab_im(y,x,2)*lab_im(y,x,2) ); } ); RMatrixXf col = r.middleCols(o,3); if( N_OBJ_COL >= 9) r.middleCols(o+6,3) = col.array().square(); o += N_OBJ_COL; // Add color difference features if( N_OBJ_COL_DIFF ) { RMatrixXf bcol = RMatrixXf::Ones( col.rows(), col.cols()+1 ); bcol.leftCols(3) = col; for( int it=0; it*3+2<N_OBJ_COL_DIFF; it++ ) { // Apply a box filter on the graph RMatrixXf tmp = bcol; for( const auto & e: g ) { tmp.row(e.a) += bcol.row(e.b); tmp.row(e.b) += bcol.row(e.a); } bcol = tmp.col(3).cwiseInverse().asDiagonal()*tmp; r.middleCols(o,3) = (bcol.leftCols(3)-col).array().abs(); o += 3; } } } if( N_OBJ_POS >= 2 ) { RMatrixXf xy = norm.asDiagonal() * bin( s, 2, [&](int x, int y){ return makeArray<2>( 1.0*x/(s.cols()-1)-0.5, 1.0*y/(s.rows()-1)-0.5 ); } ); r.middleCols(o,2) = xy; o+=2; if( N_OBJ_POS >=4 ) { r.middleCols(o,2) = xy.array().square(); o+=2; } } if( N_OBJ_EDGE ) { RMatrixXf edge_map = DirectedSobel().detect( rgb_im ); for( int j=0; j<s.rows(); j++ ) for( int i=0; i<s.cols(); i++ ) { const int id = s(j,i); int bin = edge_map(j,i)*N_OBJ_EDGE; if ( bin < 0 ) bin = 0; if ( bin >= N_OBJ_EDGE ) bin = N_OBJ_EDGE-1; r(id,o+bin) += norm[id]; } o += N_OBJ_EDGE; } const int N_BASIC = o-1; // Add in context features for( int i=0; i<N_OBJ_CONTEXT; i++ ) { const int o0 = o - N_BASIC; // Box filter the edges RMatrixXf f = RMatrixXf::Ones( Ns, N_BASIC+1 ), bf = RMatrixXf::Zero( Ns, N_BASIC+1 ); f.rightCols( N_BASIC ) = r.middleCols(o0,N_BASIC); for( Edge e: g ) { bf.row(e.a) += f.row(e.b); bf.row(e.b) += f.row(e.a); } r.middleCols(o,N_BASIC) = bf.col(0).array().max(1e-10f).inverse().matrix().asDiagonal() * bf.rightCols(N_BASIC); o += N_BASIC; } return r; }
int main(int argc, char **argv) { ros::init(argc, argv, "dml_test"); ros::NodeHandle nh; rgbd::Client client; client.intialize("/amigo/top_kinect/rgbd"); ros::Rate r(30); while (ros::ok()) { r.sleep(); rgbd::Image image; if (!client.nextImage(image)) continue; cv::Mat depth_original = image.getDepthImage(); if (!depth_original.data) continue; // - - - - - - - - - - - - - - - - - - // Downsample depth image cv::Mat depth; if (factor == 1) { depth = depth_original; } else { depth = cv::Mat(depth_original.rows / factor, depth_original.cols / factor, CV_32FC1, 0.0); for(int y = 0; y < depth.rows; ++y) { for(int x = 0; x < depth.cols; ++x) { depth.at<float>(y, x) = depth_original.at<float>(y * factor, x * factor); } } } // - - - - - - - - - - - - - - - - - - rgbd::View view(image, depth.cols); tue::Timer timer; timer.start(); EdgeMap edge_map(view); // - - - - - - - - - - - - HORIZONTAL - - - - - - - - - - - - for(int y = 0; y < depth.rows; ++y) { int x = 0; int x_start; float d_last; int x_last; // Find first valid value for(; x < depth.cols; ++x) { float d = depth.at<float>(y, x); if (d == d && d > 0) { x_start = x; x_last = x; d_last = d; break; } } for(; x < depth.cols; ++x) { int x_end = x; float d = depth.at<float>(y, x); if (d != d || d == 0) continue; bool check_edge = false; int new_x_start; if (std::abs(d - d_last) > d * 0.1) { if (d < d_last && d < max_range) edge_map.addEdgePoint(x, y, 1, 0); else if (d_last < max_range) edge_map.addEdgePoint(x_last, y, 1, 0); new_x_start = x; x_end--; check_edge = true; } else if (x_end - x_start >= max_segment_size) { check_edge = true; new_x_start = (x_start + x_end) / 2; } if (check_edge) { int x_edge = findEdgeHorizontal(y, x_start, x_end, view, edge_map); if (x_edge >= 0) x = x_edge; x_start = new_x_start; } d_last = depth.at<float>(y, x); x_last = x; } } // - - - - - - - - - - - - VERTICAL - - - - - - - - - - - - for(int x = 0; x < depth.cols; ++x) { int y = 0; int y_start; float d_last; int y_last; // Find first valid value for(; y < depth.rows; ++y) { float d = depth.at<float>(y, x); if (d == d && d > 0) { y_start = y; y_last = y; d_last = d; break; } } for(; y < depth.rows; ++y) { int y_end = y; float d = depth.at<float>(y, x); if (d != d || d == 0) continue; bool check_edge = false; int new_y_start; if (std::abs(d - d_last) > d * 0.1) { if (d < d_last && d < max_range) edge_map.addEdgePoint(x, y, 0, 1); else if (d_last < max_range) edge_map.addEdgePoint(x, y_last, 0, -1); new_y_start = y; y_end--; check_edge = true; } else if (y_end - y_start >= max_segment_size) { check_edge = true; new_y_start = (y_start + y_end) / 2; } if (check_edge) { int y_edge = findEdgeVertical(x, y_start, y_end, view, edge_map); if (y_edge >= 0) y = y_edge; y_start = new_y_start; } d_last = depth.at<float>(y, x); y_last = y; } } std::cout << std::endl; std::cout << timer.getElapsedTimeInMilliSec() << " ms" << std::endl; std::cout << "Number of edge points = " << edge_map.features.size() << std::endl; // ICP tue::Timer timer_icp; timer_icp.start(); pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>); pc->resize(edge_map.features.size()); for(unsigned int i = 0; i < edge_map.features.size(); ++i) { const EdgeFeature& f = edge_map.features[i]; pc->push_back(pcl::PointXYZ(f.point.x, f.point.y, f.point.z)); } pcl::KdTreeFLANN<pcl::PointXYZ> tree; tree.setInputCloud(pc); std::cout << "ICP: " << timer_icp.getElapsedTimeInMilliSec() << " ms" << std::endl; if (visualize) { cv::Mat canvas(depth.rows, depth.cols, CV_8UC3, cv::Scalar(50, 50, 50)); for(int i = 0; i < depth.cols * depth.rows; ++i) { float d = depth.at<float>(i); if (d == d) { int c = d / 8 * 255; canvas.at<cv::Vec3b>(i) = cv::Vec3b(c, c, c); } else canvas.at<cv::Vec3b>(i) = cv::Vec3b(100, 0, 0); } for(std::vector<EdgeFeature>::const_iterator it = edge_map.features.begin(); it != edge_map.features.end(); ++it) { const EdgeFeature& f = *it; if (f.dx == -1) canvas.at<cv::Vec3b>(f.y, f.x) = cv::Vec3b(255, 0, 0); else if (f.dx == 1) canvas.at<cv::Vec3b>(f.y, f.x) = cv::Vec3b(255, 255, 0); else if (f.dy == -1) canvas.at<cv::Vec3b>(f.y, f.x) = cv::Vec3b(0, 255, 0); else if (f.dy == 1) canvas.at<cv::Vec3b>(f.y, f.x) = cv::Vec3b(0, 255, 255); } cv::imshow("edges", canvas); cv::waitKey(3); } } ros::spin(); return 0; }