Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

asked 2016-09-15 02:26:45 -0600

ak1 gravatar image

Giving error malloc(): memory corruption: and Segementation fault

Pose.h

    #ifndef POSE_H
    #define POSE_H

    #include "opencv2/objdetect/objdetect.hpp"
    #include "opencv2/highgui/highgui.hpp"
    #include "opencv2/imgproc/imgproc.hpp"
    #include "opencv2/calib3d/calib3d.hpp"
    #include "opencv2/core/core.hpp"
    #include <iostream>
    #include <math.h>


    using namespace std;
    using namespace cv;

    class Pose
    {
    public:
    Pose();

    double* contour_detection(Mat threshold,Mat src);
    double* pose_estimation(const std::vector<cv::Point3f> &objectPoints,const std::vector<cv::Point2f> &imagePoints);
    private:
vector<Point3f> objectPoints ;
Mat  cameraIntrinsicParams ;
Mat distCoeffs;
    int index[100];
    double area_array[100];
};
    #endif // POSE_H

Pose.cpp

    #include "Pose.h"


    Pose::Pose()
    {
    objectPoints.push_back(cv::Point3f(-50.0f,-50.0f,0.0f));
objectPoints.push_back(cv::Point3f(-50.0f,50.0f,0.0f));
objectPoints.push_back(cv::Point3f(50.0f,50.0f,0.0f));
objectPoints.push_back(cv::Point3f(50.0f,-50.0f,0.0f));



    cameraIntrinsicParams=Mat(Size(3,3),CV_32FC1);


    cameraIntrinsicParams.at<float>(0,0)=  694.507824f;
cameraIntrinsicParams.at<float>(0,1)= 0 ;
cameraIntrinsicParams.at<float>(0,2)= 329.707540f;//304.729528f;
cameraIntrinsicParams.at<float>(1,0)= 0 ;
cameraIntrinsicParams.at<float>(1,1)=  694.579263f;
cameraIntrinsicParams.at<float>(1,2)= 260.350615f;//235.217420f;
cameraIntrinsicParams.at<float>(2,0)= 0 ;
cameraIntrinsicParams.at<float>(2,1)= 0 ;
cameraIntrinsicParams.at<float>(2,2)= 1 ;



   distCoeffs=Mat(Size(5,1),CV_32FC1);
   distCoeffs.at<float>(0,0)=0.015950f;
   distCoeffs.at<float>(1,0)=-0.224494f;
   distCoeffs.at<float>(2,0)=0.002544f;
   distCoeffs.at<float>(3,0)=0.000194f;
   distCoeffs.at<float>(3,0)=0.00f;
   }

   double* Pose::contour_detection(Mat threshold, Mat src)
   {
int flag =0;
double largest_area=0.0;
int largest_contour_index=0;
int lowThreshold;
int ratio=3;
int kernel_size=3;
    int const max_lowThreshold = 100;
    double *location;
    static double check[3]={0.0,0.0,0.0};
    int j=0;
    //int index[100];
    //double area_array[100];
    //double *condition;
Mat dst(src.rows,src.cols,CV_8UC1,Scalar::all(0));
Rect boundRect;
RotatedRect box;
vector<Point>Points ;
vector< vector<Point> > contours; // Vector for storing contour
vector<Vec4i> hierarchy;
Mat detected_edges;
blur(threshold, detected_edges, Size(3,3) );

    /// Canny detector
Canny( detected_edges, detected_edges, lowThreshold, lowThreshold*ratio, kernel_size );
vector< vector<Point> > contours0;
    findContours( detected_edges, contours0, hierarchy,CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE ); // Find the contours in the image

contours.resize(contours0.size());
std::vector<Point2f> imagePoints;
    std::vector<Point2f> preciseCorners(4);
    int k;

    for( k=0; k < contours0.size(); k++ )
    {
           cout<<"size"<<contours0.size()<<endl;
            //double eps = contours0[k].size() * 0.05;
    approxPolyDP(contours0[k], contours[k],9, true);

            if (contours[k].size() != 4)
    continue;
            if (!cv::isContourConvex(contours[k]))
    continue;

            if(contours[k].size()==4)
    {

    index[j]=k;
            j=j+1;

    }

     }

    if (contours0.size()==0)
    {             
    return check;
    }

if(j>1)
{

            cout<<"5"<<endl;
    for( int i = 0; i<j; i++ )
    {
              cout<<"6"<<endl;


            if(area_array[i]>largest_area)
        {
        largest_area=area_array[i];
            largest_contour_index=i;                //Store the index of largest contour
        }
    }

}   

    if(j>1) 
{    
    for( int i = 0; i<j; i++ )
            {

        double ratio =largest_area/area_array[i];

        if(ratio>=3.0 || ratio<=5.0)
                {    

            for (int c=0;c<4;c++)
            {       
            preciseCorners[c] = contours[index[largest_contour_index]][c];
            }

            cv::cornerSubPix(threshold, preciseCorners, cvSize(5,5),cvSize(-1,-1),  TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 ));
          //  cv::cornerSubPix(threshold, preciseCorners, cvSize(5,5),cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER,30,0.1));

                    for (int c=0;c<4;c++)
            {
            contours[index[largest_contour_index]][c] = preciseCorners[c];
            }

            imagePoints.push_back(Point2f(contours[index[largest_contour_index]][0].x,contours[index[largest_contour_index]][0].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][1].x,contours[index[largest_contour_index]][1].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][2].x,contours[index[largest_contour_index]][2].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][3].x,contours[index[largest_contour_index]][3].y));

            Point P1=contours[index[largest_contour_index]][0];
         Point P2=contours[index[largest_contour_index]][1];
         Point P3=contours[index[largest_contour_index]][2];
         Point P4=contours[index[largest_contour_index]][3];


         line(src,P1,P2, Scalar(0,255,0),1,CV_AA,0);
        line(src,P2,P3, Scalar(0,255,0),1,CV_AA,0);
        line(src,P3,P4, Scalar(0,255,0),1,CV_AA,0);
        line(src,P4,P1, Scalar(0,255,0),1,CV_AA,0);
            location=pose_estimation(objectPoints,imagePoints);
            return location;            

            }       
    }  
} 


      return check;     

       }

Giving error malloc(): memory corruption: and Segementation fault

Pose.h

    #ifndef POSE_H
    #define POSE_H

    #include "opencv2/objdetect/objdetect.hpp"
    #include "opencv2/highgui/highgui.hpp"
    #include "opencv2/imgproc/imgproc.hpp"
    #include "opencv2/calib3d/calib3d.hpp"
    #include "opencv2/core/core.hpp"
    #include <iostream>
    #include <math.h>


    using namespace std;
    using namespace cv;

    class Pose
    {
    public:
    Pose();

    double* contour_detection(Mat threshold,Mat src);
    double* pose_estimation(const std::vector<cv::Point3f> &objectPoints,const std::vector<cv::Point2f> &imagePoints);
    private:
vector<Point3f> objectPoints ;
Mat  cameraIntrinsicParams ;
Mat distCoeffs;
    int index[100];
    double area_array[100];
};
    #endif // POSE_H

Pose.cpp

    #include "Pose.h"


    Pose::Pose()
    {
    objectPoints.push_back(cv::Point3f(-50.0f,-50.0f,0.0f));
objectPoints.push_back(cv::Point3f(-50.0f,50.0f,0.0f));
objectPoints.push_back(cv::Point3f(50.0f,50.0f,0.0f));
objectPoints.push_back(cv::Point3f(50.0f,-50.0f,0.0f));



    cameraIntrinsicParams=Mat(Size(3,3),CV_32FC1);


    cameraIntrinsicParams.at<float>(0,0)=  694.507824f;
cameraIntrinsicParams.at<float>(0,1)= 0 ;
cameraIntrinsicParams.at<float>(0,2)= 329.707540f;//304.729528f;
cameraIntrinsicParams.at<float>(1,0)= 0 ;
cameraIntrinsicParams.at<float>(1,1)=  694.579263f;
cameraIntrinsicParams.at<float>(1,2)= 260.350615f;//235.217420f;
cameraIntrinsicParams.at<float>(2,0)= 0 ;
cameraIntrinsicParams.at<float>(2,1)= 0 ;
cameraIntrinsicParams.at<float>(2,2)= 1 ;



   distCoeffs=Mat(Size(5,1),CV_32FC1);
   distCoeffs.at<float>(0,0)=0.015950f;
   distCoeffs.at<float>(1,0)=-0.224494f;
   distCoeffs.at<float>(2,0)=0.002544f;
   distCoeffs.at<float>(3,0)=0.000194f;
   distCoeffs.at<float>(3,0)=0.00f;
   }

   double* Pose::contour_detection(Mat threshold, Mat src)
   {
int flag =0;
double largest_area=0.0;
int largest_contour_index=0;
int lowThreshold;
int ratio=3;
int kernel_size=3;
    int const max_lowThreshold = 100;
    double *location;
    static double check[3]={0.0,0.0,0.0};
    int j=0;
    //int index[100];
    //double area_array[100];
    //double *condition;
Mat dst(src.rows,src.cols,CV_8UC1,Scalar::all(0));
Rect boundRect;
RotatedRect box;
vector<Point>Points ;
vector< vector<Point> > contours; // Vector for storing contour
vector<Vec4i> hierarchy;
Mat detected_edges;
blur(threshold, detected_edges, Size(3,3) );

    /// Canny detector
Canny( detected_edges, detected_edges, lowThreshold, lowThreshold*ratio, kernel_size );
vector< vector<Point> > contours0;
    findContours( detected_edges, contours0, hierarchy,CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE ); // Find the contours in the image

contours.resize(contours0.size());
std::vector<Point2f> imagePoints;
    std::vector<Point2f> preciseCorners(4);
    int k;

    for( k=0; k < contours0.size(); k++ )
    {
           cout<<"size"<<contours0.size()<<endl;
            //double eps = contours0[k].size() * 0.05;
    approxPolyDP(contours0[k], contours[k],9, true);

            if (contours[k].size() != 4)
    continue;
            if (!cv::isContourConvex(contours[k]))
    continue;

            if(contours[k].size()==4)
    {

    index[j]=k;
            j=j+1;

    }

     }

    if (contours0.size()==0)
    {             
    return check;
    }

if(j>1)
{

            cout<<"5"<<endl;
    for( int i = 0; i<j; i++ )
    {
              cout<<"6"<<endl;

                     area_array[i]=contourArea( contours[index[i]],false);
            if(area_array[i]>largest_area)
        {
        largest_area=area_array[i];
            largest_contour_index=i;                //Store the index of largest contour
        }
    }

}   

    if(j>1) 
{    
    for( int i = 0; i<j; i++ )
            {

        double ratio =largest_area/area_array[i];

        if(ratio>=3.0 || ratio<=5.0)
                {    

            for (int c=0;c<4;c++)
            {       
            preciseCorners[c] = contours[index[largest_contour_index]][c];
            }

            cv::cornerSubPix(threshold, preciseCorners, cvSize(5,5),cvSize(-1,-1),  TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 ));
          //  cv::cornerSubPix(threshold, preciseCorners, cvSize(5,5),cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER,30,0.1));

                    for (int c=0;c<4;c++)
            {
            contours[index[largest_contour_index]][c] = preciseCorners[c];
            }

            imagePoints.push_back(Point2f(contours[index[largest_contour_index]][0].x,contours[index[largest_contour_index]][0].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][1].x,contours[index[largest_contour_index]][1].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][2].x,contours[index[largest_contour_index]][2].y));
        imagePoints.push_back(Point2f(contours[index[largest_contour_index]][3].x,contours[index[largest_contour_index]][3].y));

            Point P1=contours[index[largest_contour_index]][0];
         Point P2=contours[index[largest_contour_index]][1];
         Point P3=contours[index[largest_contour_index]][2];
         Point P4=contours[index[largest_contour_index]][3];


         line(src,P1,P2, Scalar(0,255,0),1,CV_AA,0);
        line(src,P2,P3, Scalar(0,255,0),1,CV_AA,0);
        line(src,P3,P4, Scalar(0,255,0),1,CV_AA,0);
        line(src,P4,P1, Scalar(0,255,0),1,CV_AA,0);
            location=pose_estimation(objectPoints,imagePoints);
            return location;            

            }       
    }  
} 


      return check;     

       }

pose_estimaton function

       double* Pose::pose_estimation(const std::vector<cv::Point3f> &objectPoints,const std::vector<cv::Point2f> &imagePoints)
        {

       static double position[4];

       Mat rvec,tvec,inliers;
   //rvec=Mat::zeros(3,1,CV_64FC1);
    //tvec=Mat::zeros(3,1,CV_64FC1);
   //int flags=0;

 bool useExtrinsicGuess =false; 

     int method = CV_ITERATIVE ;

      cv::solvePnP(objectPoints,imagePoints, cameraIntrinsicParams, distCoeffs,rvec, tvec,useExtrinsicGuess,method);

     Mat distant=Mat(Size(3,3),CV_64FC1);
     Mat jacobian=Mat(Size(3,1),CV_32FC1);

 Rodrigues(rvec,distant,jacobian);

     Mat J;
 vector<Point2f> p(4);
 projectPoints(objectPoints,rvec,tvec, cameraIntrinsicParams,  distCoeffs, p, J);

     float sum = 0.;
for (size_t i = 0; i <p.size(); i++)
     {
      sum += sqrt((p[i].x - imagePoints[i].x)* (p[i].x - imagePoints[i].x)+(p[i].y - imagePoints[i].y)* (p[i].y - imagePoints[i].y));
     }



    Mat R= distant.t(); 

    cout<<""<<endl;

           //if (sum < 3000)
           //{
           cout << "sum=" << sum << endl;
           Mat T = -R * tvec ;
           position[0] =T.at<double>(0,0);
           position[1]=T.at<double>(1,0);
           position[2]=T.at<double>(2,0);
           //}         
    return position;
     }
Copyright OpenCV foundation, 2012-2018. Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2