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;
}
