espee's profile - activity

I have been trying to estimate the 6D pose of a moving camera (in 6D) using opencv library. My steps are:

1. use goodFeaturesToTrack to get features.
2. calculate optical flow
3. track features
4. get fundamental matrix
5. get essential matrix
6. check the combination of R or t to determine the true R & t using triangulation (could find a better way to do this.. had to combine R & t and then later extract R & t)
7. Use the height information (from altitude sensor) to get scale factor and scale the translation
8. Update the transformation
9. Save the new feature set and image for next loop.

But the results are far from expectation. I verified that R is orthogonal. But cant figure out where I went wrong. My code is as follows:

void imageCb(const sensor_msgs::ImageConstPtr& image) 
{
    try
      {
    if(start_flag_)
      {
        cv_ptr_prev_ = cv_bridge::toCvCopy(image, enc::BGR8);
      }
        cv_ptr_new_ = cv_bridge::toCvCopy(image, enc::BGR8);
      }
    catch (cv_bridge::Exception& e)
      {
    ROS_ERROR("Conversion error: %s", e.what());
    return;
      }
    if(start_flag_)
      {
    image_prev_.create(cv_ptr_prev_->image.rows, cv_ptr_prev_->image.cols , CV_8UC1);
    image_new_.create(cv_ptr_new_->image.rows, cv_ptr_new_->image.cols , CV_8UC1);
      }

    cvtColor( cv_ptr_new_->image, image_new_, CV_BGR2GRAY);
    cvtColor( cv_ptr_prev_->image, image_prev_, CV_BGR2GRAY);

    Mat F, E;                //Fundamental and Essential Matrices
    vector<Point2f> p1;      //common feature points in first image
    vector<Point2f> p2;      //common feature points in second image
    double f = 320;          //camera parameter focal length
    double Cx = 320.5;       //Resolution
    double Cy = 240.5;
    Mat K = (Mat_<double>(3,3) << f, 0 , -Cx, 0, f, -Cy, 0, 0, 1); //camera internal matrix

    Mat W = (Mat_<double>(3,3) << 0, -1, 0, 1, 0, 0, 0, 0 , 1);   //for SVD
    Mat Z = (Mat_<double>(3,3) << 0, 1, 0, -1, 0, 0, 0, 0 , 1);  //for SVD

    Mat w, u , vt; //SVD result of Essential Matrix
    Mat tx, Ra, Rb; //Temporary checking for R and t
    Mat t_unscaled, t_scaled, R;  //True R and t for the current step

    //Lucas Kanade Optical Flow

    vector<uchar> features_found;
    if(!start_flag_)
      {
    calcOpticalFlowPyrLK(image_prev_, image_new_, features_prev_, features_new_, features_found, noArray(),Size(win_size_*4+1, win_size_*4+1), 5,TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20, 0.3));
        for( int i = 0; i < (int)features_new_.size(); i++ )
      {
        circle(cv_ptr_prev_->image, features_new_[i],1,CV_RGB(255,255,255));
        circle(cv_ptr_prev_->image, features_prev_[i],1,CV_RGB(0,0,0));
        if( !features_found[i] )
          continue;
        else
          {
        line(cv_ptr_prev_->image, features_prev_[i], features_new_[i], Scalar(255,0,0), 1, CV_AA);
        p1.push_back(Point2f(features_prev_[i].x-Cx, features_prev_[i].y-Cy));
        p2.push_back(Point2f(features_new_[i].x-Cx, features_new_[i].y-Cy));
          }  
      }
    image_pub_.publish(cv_ptr_prev_->toImageMsg());          //publish image with features

    //Estimation
    vector<uchar> inliers;

    if(p1.size()==p2.size())
      {
        F =  findFundamentalMat(p1, p2, CV_FM_RANSAC,1.0, 0.999, inliers);
        //      cout<<"Number of inliers is::::::"<<sum(inliers)<<endl;
        E = K.t()*F*K;
        SVD::compute(E, w, u, vt);

        tx = u * Z * u.t();     Ra = u * W * vt;        Rb = u * W.t() * vt;

        if(determinant(Ra)<=0)        Ra = -Ra;
        if(determinant(Rb)<=0)        Rb = -Rb;

        t_unscaled = (Mat_<double>(1,3) << tx.at<double>(2,1), tx.at<double>(0,2), tx.at<double>(1,0) );

        Mat tminus = -(t_unscaled.clone());

        //Check which Ra or Rb or t

        vector<Mat> R_vec ...

Hello,

I am trying to process image outside callback. Its a Harris detector using opencv but I am running into segmentation fault error.

my code is as follows;

namespace enc = sensor_msgs::image_encodings;
class harris_node
{
  protected:
  //   image_transport::ImageTransport it_;
image_transport::Subscriber sub_;
image_transport::Publisher pub_;
ros::NodeHandle nh_;
ros::NodeHandle nhPrivate_;

sensor_msgs::ImageConstPtr imageIn_;
cv_bridge::CvImagePtr image_in;   //pointer to input image
cv_bridge::CvImagePtr image_out;  //for ros msg conversion 
cv::Mat cvImage_in;       //for processing, input at each step
cv::Mat cvImage_out;      //for processing, output at each step
int thresh;
int max_thresh;
int blockSize;
int apertureSize;
double k;


 public:
 harris_node(ros::NodeHandle& nh, ros::NodeHandle& nhPrivate):nh_(nh), nhPrivate_(nhPrivate), thresh(200), max_thresh(255), blockSize(2), apertureSize(3), k(0.04)
 {
    image_transport::ImageTransport it_(nh_);
    sub_ = it_.subscribe("front_cam/camera/image", 100, &harris_node::imageCb, this);
    pub_=it_.advertise("front_cam/camera/image_processed", 1);
 }

 void imageCb(const sensor_msgs::ImageConstPtr& image)
 {  

    try                         
    {
        imageIn_ = image;        
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Conversion error: %s", e.what());
        return;
    }
 }

 void harris_detector()
 {
    image_in = cv_bridge::toCvCopy(imageIn_, enc::BGR8);
    cvImage_in = image_in->image;       
    cvtColor(cvImage_in, cvImage_out, CV_8UC1); 
    cvImage_in = cvImage_out;
    cornerHarris(cvImage_in, cvImage_out, blockSize, apertureSize, k, cv::BORDER_DEFAULT);    //Harris detector
    cvImage_in = cvImage_out;
    normalize(cvImage_in, cvImage_out, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());       //normalize
    cvImage_in = cvImage_out;
    convertScaleAbs (cvImage_in, cvImage_out); //scaled


    //Drawing circles around corner

    for(int j=0; j<cvImage_out.rows ; j++)
    {
        for(int i = 0; i<cvImage_out.cols; i++)
        {
            if((int) cvImage_out.at<float>(j,i) > thresh)
            {
                circle(cvImage_out, cv::Point(j,i),5,cv::Scalar(0),2,8,0);
            }
        }

    }
 }


 void spin()
 {
    ros::Rate rate(30);
    while (ros::ok())
    {
        spinOnce();
        rate.sleep();
    }
 }

 virtual void spinOnce()
 {
    harris_detector();
    spin();
 }
 };

 class harris_node_with_gui:public harris_node
 {
public:
harris_node_with_gui(ros::NodeHandle& nh, ros::NodeHandle& nhPrivate):   harris_node(nh, nhPrivate){}
void spinOnce()
{
    harris_node::spinOnce();
    cv::imshow("inputImage", cvImage_in);
    cv::imshow("Corners", cvImage_out); 
}
   };




 int main(int argc, char** argv)
 {
 ros::init(argc, argv, "image_processor"); 
 ros::NodeHandle nh;
 ros::NodeHandle nhPrivate("~");

 harris_node* node = 0;
 node = new harris_node_with_gui (nh, nhPrivate);
 node->spinOnce();
 }

gdb debugging shows Program received signal SIGSEGV, Segmentation fault.

on backtrace I get:

#0  0xb7fdd424 in __kernel_vsyscall ()
#1  0xb74b81df in raise () from /lib/i386-linux-gnu/libc.so.6
#2  0xb74bb825 in abort () from /lib/i386-linux-gnu/libc.so.6
#3  0xb74b1085 in ?? () from /lib/i386-linux-gnu/libc.so.6
#4  0xb74b1137 in __assert_fail () from /lib/i386-linux-gnu/libc.so.6
#5  0xb7f74555 in    cv_bridge::toCvCopy(boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, std::string const&) ()
 from /opt/ros/hydro/lib/libcv_bridge.so
#6  0x080532bb in harris_node::harris_detector() ()
#7  0x0805372f in harris_node::spinOnce() ()
#8  0x08053778 in harris_node_with_gui::spinOnce() ()
#9  0x080536e1 in harris_node::spin() ()
#10 0x08050f2a in main ()

Any help is appreciated..

Edited

After I swap i and j the backtrace output is still the same but I get another error:

image_processing: /usr/include/boost/smart_ptr/shared_ptr.hpp:412:    boost::shared_ptr<T>::reference boost::shared_ptr<T>::operator*() const [with T = const    sensor_msgs::Image_<std::allocator<void> >, boost::shared_ptr<T>::reference = const   sensor_msgs::Image_<std::allocator<void> >&]: Assertion `px != 0' failed.

So I think I have initialized the pointers in the class to NULL value and this has ... (more)