$search

c_track_features.cpp

Go to the documentation of this file.

#define NO_ROS 0

#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#include <Eigen/Eigen>
#include <assert.h>
#include <iostream>
#include <set>
#include <opencv2/core/eigen.hpp>
#include <cmath>
#include <ros/ros.h>
#include "sensor_msgs/Image.h"
#include "cv_bridge/CvBridge.h"
#include "camera_self_filter/mask.h"
#include "Timer.h"
#include <fstream>
#include <iostream>
//#include <boost/math/distributions/beta.hpp>

using std::cout;
using std::endl;


/***
 * Retrives rotation and translation minimzing the error of the rigid transformation between two sets of points.
 * Modified opencv version.
 */
static void
icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b,
                int count, CvMat* M )
{


        double sa[16], sb[4], m[4], *om = M->data.db;
        CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb );
        CvMat MM = cvMat( 4, 1, CV_64F, m );

        int i;

        memset( sa, 0, sizeof(sa) );
        memset( sb, 0, sizeof(sb) );

        for( i = 0; i < count; i++ )
        {
            sa[0] += a[i].x*a[i].x + a[i].y*a[i].y;
            sa[1] += 0;
            sa[2] += a[i].x;
            sa[3] += a[i].y;

            sa[4] += 0;
            sa[5] += a[i].x*a[i].x + a[i].y*a[i].y;
            sa[6] += -a[i].y;
            sa[7] += a[i].x;

            sa[8] += a[i].x;
            sa[9] += -a[i].y;
            sa[10] += 1;
            sa[11] += 0;

            sa[12] += a[i].y;
            sa[13] += a[i].x;
            sa[14] += 0;
            sa[15] += 1;

            sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
            sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
            sb[2] += b[i].x;
            sb[3] += b[i].y;
        }

        cvSolve( &A, &B, &MM, CV_SVD );

        om[0] = om[4] = m[0];
        om[1] = -m[1];
        om[3] = m[1];
        om[2] = m[2];
        om[5] = m[3];

}

/***
 * Retrives rotation and translation minimzing the error of the rigid transformation between two sets of points.
 *
 */
cv::Mat cvEstimateRigidTransformFrom2( const cv::Mat& A, const cv::Mat& B) {
         cv::Mat M = cv::Mat(2, 3, CV_64F);

         CvPoint2D32f* a = (CvPoint2D32f*) A.data;
         CvPoint2D32f* b = (CvPoint2D32f*) B.data;
         CvMat matM = M;
         icvGetRTMatrix(a,b, 2,  &matM);
         return M;
}


/***
 * Extracts corner features from an image  and tracks them in subsequent images.
 * The features are clustered such that each feature cluster consists of features with similar trajectories, i.e.
 * which can be explained by the same sequence of rigid transforms.
 */

class FeatureTracker{
public:

        //Parameters for feature detection
        static const int MAX_CORNERS = 500;
        static const double quality = 0.01;
        static const double min_distance = 10;
        static const int WIN_SIZE = 10;

        //number of points used to calculate RT
        const static int num_points_to_select = 3;

        cv::Mat vis_image;
        cv::Mat vis_image_orig;
        cv::Mat last_image;
        std::vector<std::vector<cv::Point2f> > corner_trajectories;
        std::vector<std::vector<cv::Point2f> > corner_trajectories_filtered_and_transposed;

        Eigen::VectorXi good_corners;

        //executed tracking steps
        int num_tracking_steps;

    // colormap for disparities, RGB order
    static unsigned char colormap[];

    static const float residual_error_comp =  0.1 ;// 0.1; //deduct half a pixel from the residuals at each timestep

        FeatureTracker():num_tracking_steps(0){}

        /***
         * Find corners using OpnecCV's goodFeaturesToTrack.
         */
        std::vector<cv::Point2f> findCorners(cv::Mat& first_image, cv::Mat* mask = 0);

        /***
         * Execute one tracking step:
         *      Calculate sparse optical flow from all corners in old image
         */
        bool tracking_step(cv::Mat& new_image);

        /***
         * Caluculates residuals as squared reprojection error between  rigid transform trajectory predicted by hypothesis and actual trajectory.
         */
        bool calcResiduals(const std::vector<Eigen::Matrix3f>& RTs, const std::vector<std::vector<cv::Point2f> >& corner_trajectories_t, Eigen::VectorXf& residuals );

        bool filterTrajectories();

        int draw(Eigen::VectorXf& prob_dist);

        bool selectRandomFeatures(std::vector<int>& features_idx, const Eigen::VectorXf& mask);

   /***
    * generates a rotation translation trajectory hypothesis for all features marked in mask in a RANSAC like fashion.
    */
        bool generateHypothesis(const Eigen::VectorXf& mask, std::vector<Eigen::Matrix3f>& RTs );

        bool writeClustersToFile(int& idx,cv::Point2f& feature,std::ofstream& a_file);

    /*** Top-level function:
     *  Samples a number of hypothesis sets, i.e. samples sets of possible feature clusters.
     *  Hypothesis sets are scored and best one is returned/visualized.
     */
        bool evluateTrajectories();


        /***
         * Sample recursively possible feature-to-cluster associations (hypotheses) by generating a hypothesis
         * on the input features set, calculate outliers and generate another hypothesis, ... and so on until a
         * hypothesis exists for each feature.
         */
        float sampleHypothesisSet(std::vector<int>& feature_assocoation);

};

/***
 * Find corners using OpnecCV's goodFeaturesToTrack.
 */
std::vector<cv::Point2f> FeatureTracker::findCorners(cv::Mat& first_image, cv::Mat* mask){
        num_tracking_steps = 0;
        cv::Mat first_image_grey;
        cv::cvtColor(first_image, first_image_grey, CV_BGR2GRAY);
        std::vector<cv::Point2f> corners;
        cv::goodFeaturesToTrack(first_image_grey,corners, MAX_CORNERS, quality,min_distance);

        good_corners = Eigen::VectorXi::Ones(corners.size());
        cv::cornerSubPix(first_image_grey, corners, cv::Size(WIN_SIZE, WIN_SIZE), cv::Size(-1,-1), cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
        corner_trajectories.push_back(corners);
        last_image = first_image_grey;

        //visulization
        vis_image_orig =first_image;
        vis_image = vis_image_orig.clone();
        for (unsigned int i = 0; i< corners.size(); i++){
                int color_idx = ((num_tracking_steps * 11) % 256) * 3;
                cv::circle(vis_image, corners[i], 3, cv::Scalar(colormap[color_idx], colormap[color_idx +1], colormap[color_idx +2],0), -1);
        }

        return corners;
}

/***
 * Execute one tracking step:
 *      Calculate sparse optical flow from all corners in old image
 */
bool FeatureTracker::tracking_step(cv::Mat& new_image){
        num_tracking_steps++;
        cv::Mat new_image_grey;
        cv::cvtColor(new_image, new_image_grey, CV_BGR2GRAY);
        std::vector<cv::Point2f> new_corners;
        std::vector<unsigned char> status;
        std::vector<float> error;
        cv::calcOpticalFlowPyrLK(last_image, new_image_grey, corner_trajectories.back(), new_corners, status, error, cv::Size(WIN_SIZE,WIN_SIZE), 3, cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));

        last_image = new_image_grey;

        corner_trajectories.push_back(new_corners);
        for (unsigned int i = 0; i< good_corners.rows(); i++){
                good_corners[i] =  good_corners[i] & status[i];
        }
        printf("num good corners %d  %d new _corner_size %d \n", good_corners.sum(), good_corners.rows(), corner_trajectories.back().size());

        //visulization
        vis_image_orig =new_image;
        vis_image = vis_image_orig.clone();
        std::vector<cv::Point2f>& old_corners = corner_trajectories[num_tracking_steps - 1];
        for (unsigned int i = 0; i< new_corners.size(); i++){
                if (!good_corners[i]) continue;
                int color_idx = ((num_tracking_steps * 11) % 256) * 3;
//                      cv::circle(vis_image, new_corners[i], 3, cv::Scalar(colormap[color_idx], colormap[color_idx +1], colormap[color_idx +2],0), -1);
//                      cv::line(vis_image,new_corners[i], old_corners[i], cv::Scalar(0,0,255,0), 1);
//                      cv::line(vis_image,new_corners[i], old_corners[i], cv::Scalar(colormap[color_idx], colormap[color_idx +1], colormap[color_idx +2]), 1);

        }

        return true;


}


/***
 * Caluculates residuals as squared reprojection error between  rigid transform trajectory predicted by hypothesis and actual trajectory.
 */
bool FeatureTracker::calcResiduals(const std::vector<Eigen::Matrix3f>& RTs, const std::vector<std::vector<cv::Point2f> >& corner_trajectories_t, Eigen::VectorXf& residuals ){

        residuals = Eigen::VectorXf(corner_trajectories_t.size());
        for (unsigned int t = 0; t < corner_trajectories_t.size(); t++){
                const std::vector<cv::Point2f>& current_trajectory = corner_trajectories_t[t];
//                      printf("RTs.size() %d current_trajectory.size() %d\n",RTs.size(),current_trajectory.size());
                assert(RTs.size() == current_trajectory.size() - 1);
                float residual = 0;
                for (unsigned int c = 0; c < current_trajectory.size() - 1; c++){
                        Eigen::Vector3f old_corner (current_trajectory[c].x, current_trajectory[c].y, 1.0);
                        Eigen::Vector3f new_corner (current_trajectory[c+1].x, current_trajectory[c+1].y, 1.0);
                        Eigen::Vector3f res = new_corner - (RTs[c] * old_corner);
//                              cout<<"res for traj "<<t<<": vec\n"<<res<<endl;

                        residual += std::max(0.0f,res.squaredNorm() - residual_error_comp);

                }
                residuals[t] = residual;
        }
        return true;
}


bool FeatureTracker::filterTrajectories(){

        std::vector<std::vector<cv::Point2f> >  corner_trajectories_filtered;
        printf("corner_trajectories size %d corner_trajectories[0].size() %d good _corners %d\n", corner_trajectories.size(), corner_trajectories[0].size(), good_corners.rows());
        int num_good_features = good_corners.sum();

//filter out bad features
        for (unsigned int j=0; j < corner_trajectories.size(); j++){
                std::vector<cv::Point2f> features_filtered;
                features_filtered.reserve(num_good_features);
                for (unsigned int i=0; i < good_corners.rows(); i++){
                        if (good_corners[i]){
                                features_filtered.push_back(corner_trajectories[j][i]);
                        }
                }
                corner_trajectories_filtered.push_back(features_filtered);
        }

//transpose
        corner_trajectories_filtered_and_transposed.clear();
        corner_trajectories_filtered_and_transposed.reserve(good_corners.rows());
        for (unsigned int i=0; i < good_corners.rows(); i++){
                std::vector<cv::Point2f> features_trans;
                features_trans.reserve(corner_trajectories_filtered.size());
                for (unsigned int j=0; j < corner_trajectories_filtered.size(); j++){
                         features_trans.push_back(corner_trajectories_filtered[j][i]);
                }
                corner_trajectories_filtered_and_transposed.push_back(features_trans);
        }

        printf("corner_trajectories_filtered size %d \n", corner_trajectories_filtered.size());

        return true;

}


int FeatureTracker::draw(Eigen::VectorXf& prob_dist){
        float pos = (float) rand() / RAND_MAX;
        int idx = 0;
        float accumulator = 0.0f;
        for (;;){
                accumulator += prob_dist[idx];
                if (accumulator > pos) break;
                idx++;
        }
        return idx;

}


//TODO: This needs to be done more efficiently...
bool FeatureTracker::selectRandomFeatures(std::vector<int>& features_idx, const Eigen::VectorXf& mask){

        float variance = 200.0f * 200.0f; //pixel size of objects
        //select random but depending on gaussian distance
        //first create map
        int num_non_masked_elements = mask.sum();
        if (num_non_masked_elements < 3){
                printf("num non masked elements %d\n", num_non_masked_elements);
                return false;
        }
        int* map_array = new int[num_non_masked_elements];
        int counter = 0;
        for (int i = 0; i < mask.rows() ; i++){
                if (mask[i] > 0.0){
                        map_array[counter] = i;
                        counter++;
                }


        }

        //we assume 3 points are selected...
        int idx1 = rand() % num_non_masked_elements;
        Eigen::VectorXf prob_dist(num_non_masked_elements);
        cv::Point2f current_point = corner_trajectories_filtered_and_transposed[map_array[idx1]].back();
        for(int i=0; i<num_non_masked_elements; i++){
                if (i==idx1){
                        prob_dist[i] = 0.0;
                        continue;
                }
                cv::Point2f other = corner_trajectories_filtered_and_transposed[map_array[i]].back();
                float dist_sqr = pow((other.x - current_point.x ),2) +  pow((other.y - current_point.y ),2);
                printf("dist sqr %d %f\n", i, dist_sqr );
                float dist_sqr_n = dist_sqr / variance;
                prob_dist[i] = exp( -0.5 * dist_sqr_n);

        }

        cout<<"prob_dist\n"<<prob_dist<<endl;
        float normalizer =  prob_dist.sum();

        //take care of the case where numbers are really low
        if (normalizer < 0.0000000001){
                return false;
        }

        prob_dist = prob_dist / normalizer;
        cout<<"prob_dist_n\n"<<prob_dist<<endl;
        int idx2 = draw(prob_dist);

        assert(map_array[idx1] < mask.rows());
        assert(map_array[idx2] < mask.rows());
        features_idx.clear();
        features_idx.push_back(map_array[idx1]);
        features_idx.push_back(map_array[idx2]);


        delete [] map_array;
        return true;
}


/***
 * generates a rotation translation trajectory hypothesis for all features marked in mask in a RANSAC like fashion.
 */
bool FeatureTracker::generateHypothesis(const Eigen::VectorXf& mask, std::vector<Eigen::Matrix3f>& RTs ){


        int num_elements = mask.sum();
        printf("in generateHypothesis num_elements %d\n",num_elements);
        if (num_elements < 3) return false;

        std::vector<int> features_idx;
        if (!selectRandomFeatures(features_idx, mask)) return false;

        RTs.reserve(num_tracking_steps);
        printf("num_tracking_steps %d\n",num_tracking_steps);
        for (int i = 1; i< num_tracking_steps+1; i++){
                printf("step gen %d \n", i);
                cv::Mat new_points (features_idx.size(), 1, CV_32FC2);
                cv::Mat old_points (features_idx.size(), 1, CV_32FC2);
                for (int j = 0; j < features_idx.size(); j++){
                        cv::Point2f& new_point = corner_trajectories_filtered_and_transposed[features_idx[j]][i];
                        new_points.ptr<float>()[j * 2] = new_point.x;
                        new_points.ptr<float>()[j * 2 + 1] = new_point.y;
                        cv::Point2f& old_point = corner_trajectories_filtered_and_transposed[features_idx[j]][i-1];
                        old_points.ptr<float>()[j * 2] = old_point.x;
                        old_points.ptr<float>()[j * 2 + 1] = old_point.y;
                }
//                      cv::Mat RT_cv = cv::estimateRigidTransform(old_points, new_points, false);
                cv::Mat RT_cv = cvEstimateRigidTransformFrom2(old_points, new_points);
                Eigen::Matrix3f RT = Eigen::Matrix3f::Identity();
                Eigen::Matrix<float, 2,3> rt_temp;
                cv::cv2eigen(RT_cv, rt_temp);
                RT.block<2,3>(0,0) = rt_temp;
                RTs.push_back(RT);
        }
        cout<<"last hyp tf \n"<<RTs.back()<<endl;
        return true;


}

bool FeatureTracker::writeClustersToFile(int& idx,cv::Point2f& feature,std::ofstream& a_file){


                //std::stringstream ss;
                //ss<<ros::Time::now();
                //std::ofstream a_file ( ss.str().c_str() );
            a_file<< feature.x << " " << feature.y << " " << idx << std::endl;
        


}


/*** Top-level function:
 *  Samples a number of hypothesis sets, i.e. samples sets of possible feature clusters.
 *  Hypothesis sets are scored and best one is returned/visualized.
 */
bool FeatureTracker::evluateTrajectories(){

        int num_samples = 15;
        int num_hypotheses = 10;
        filterTrajectories();
        Eigen::VectorXf scores = Eigen::VectorXf::Zero(num_samples);
        std::vector<std::vector<int > > feature_assocoations;

        //create hypothesis set samples
        for (int i = 0; i<num_samples; i++){
                std::vector<int> feature_assocoation;
                scores[i] = sampleHypothesisSet(feature_assocoation);
                feature_assocoations.push_back(feature_assocoation);
//                      if(feature_assocoation.size() != 1){
//                              cv::imshow("tracker_temp", vis_image);
//                              cvWaitKey(1500);
//                      }
        }

        //find best hypothesis...
        int best_hypthesis_set_idx;
        scores.minCoeff(&best_hypthesis_set_idx);

        //..which becomes final feature-to-cluster association.
        std::vector<int>& final_feature_association = feature_assocoations[best_hypthesis_set_idx];

        vis_image = vis_image_orig.clone();

//only for logging purposes
                std::stringstream ss;
                ss<<ros::Time::now();
                std::ofstream a_file ( ss.str().c_str() );

        for (int i = 0; i < corner_trajectories_filtered_and_transposed.size(); i++){
                int idx = final_feature_association[i];
                
                writeClustersToFile(idx,corner_trajectories_filtered_and_transposed[i].back(),a_file);


                if (idx >= 0){
                        cv::circle(vis_image, corner_trajectories_filtered_and_transposed[i].back(), 9, cv::Scalar( colormap[3*idx], colormap[3*idx+1], colormap[3*idx+2],0), 5);

                
//                      }else{
//                              cv::circle(vis_image, corner_trajectories_filtered_and_transposed[i].back(), 9, cv::Scalar(  0 , 0, 0, 0), 5, 1);
                }
        }
        
        a_file.close();

        return true;

}


/***
 * Sample recursively possible feature-to-cluster associations (hypotheses) by generating a hypothesis
 * on the input features set, calculate outliers and generate another hypothesis, ... and so on until a
 * hypothesis exists for each feature.
 */
float FeatureTracker::sampleHypothesisSet(std::vector<int>& feature_assocoation){
        int num_hypotheses = 10;
        float mask_threshold = 80;

        Eigen::MatrixXf residualMatrix (corner_trajectories_filtered_and_transposed.size(), num_hypotheses);

        std::vector<std::vector<Eigen::Matrix3f> > hypothesis;

        //create identity Matrix as first hypothesos
        std::vector<Eigen::Matrix3f> RTs_I;
        RTs_I.resize(num_tracking_steps, Eigen::Matrix3f::Identity());
        printf ("num_tracking_steps %d RTs_I.size() %d \n", num_tracking_steps, RTs_I.size());
        hypothesis.push_back(RTs_I);

        Eigen::VectorXf mask = Eigen::VectorXf::Ones(corner_trajectories_filtered_and_transposed.size());
        bool points_left;
        int i=1;
        for (; i<num_hypotheses; i++){
                Eigen::VectorXf residuals;
                std::vector<Eigen::Matrix3f>& last_hypothesis = hypothesis[i-1];
                printf("last_hypothesis size %d\n", last_hypothesis.size());

                //calculate the residuals incurred by current hypothesis (i.e. error between features' actual RT and the hypothetical RT).
                calcResiduals(last_hypothesis, corner_trajectories_filtered_and_transposed , residuals);

                //store residuals
                residualMatrix.col(i-1) = residuals;

//                      cout<<"residuals\n"<<residuals<<endl;
//                      cout<<"residuals_mat \n"<<residualMatrix<<endl;

                //find inliers and mark them in binary mask
                Eigen::VectorXf mask_bin = (residuals.array() - mask_threshold).matrix();
                for (int i = 0; i< mask_bin.rows(); i++){
                        mask_bin[i] = (mask_bin[i] > 0.0f) ? 1.0f : 0.0f;
                }

                Eigen::ArrayXf mask_array= mask.array();
                Eigen::ArrayXf mask_array_bin= mask_bin.array();


                mask = mask_array.min(mask_array_bin);

//              mask = mask.array().min(mask_bin.array());

                std::vector<Eigen::Matrix3f> new_hypothesis;

                //for the outliers, generate a new hypothesis
                points_left = generateHypothesis(mask, new_hypothesis );
                if (!points_left) break;
                printf("generated new hyp of size %d\n", new_hypothesis.size());
                hypothesis.push_back(new_hypothesis);

        }
        //calc last residuals if all hypothesis had to be used
        int& actual_num_hypothesis = i;
        printf("actual_num_hypothesis %d num_hypotheses %d\n", actual_num_hypothesis,num_hypotheses);
        if (actual_num_hypothesis == num_hypotheses){
                Eigen::VectorXf residuals;
                printf("ypothesis.back() size %d\n", hypothesis.back().size());
                calcResiduals(hypothesis.back(), corner_trajectories_filtered_and_transposed ,residuals );
//                      cout<<"last residuals\n"<<residuals;
                residualMatrix.col(residualMatrix.cols()-1) = residuals;
        }else{
//                      cout<<"residuals_mat \n"<<residualMatrix<<endl;
                Eigen::MatrixXf residualMatrix2 = residualMatrix.block(0,0, residualMatrix.rows(), actual_num_hypothesis);
                residualMatrix = residualMatrix2;
//                      cout<<"residuals_mat \n"<<residualMatrix<<endl;
        }


        cout<<"residuals_mat \n"<<residualMatrix<<endl;

        Eigen::VectorXf summed_residuals = residualMatrix.colwise().sum();

        cout<<"summed_residuals \n"<<summed_residuals<<endl;

        Eigen::VectorXf summed_residuals_per_h = Eigen::VectorXf::Zero(residualMatrix.cols());


        //Finally run the process again and assign each feature to its best possible RT hypothesis (or mark it as complete outlier).
        std::vector<int> best_feature_h_idx;
        best_feature_h_idx.resize(corner_trajectories_filtered_and_transposed.size(),0);
        Eigen::VectorXf feature_per_hyp_count = Eigen::VectorXf::Ones(residualMatrix.cols());
        feature_per_hyp_count *= 0.0000001f;
        int num_outliers = 0;
        for (int i = 0; i < corner_trajectories_filtered_and_transposed.size(); i++){
                int idx;
                residualMatrix.row(i).minCoeff(&idx);
                float residual_value = residualMatrix(i,idx);

                //check if feature was an outlier
                if (mask[i] > 0.0f){
                        num_outliers++;
                        best_feature_h_idx[i] = -1;
                        continue;
                }

                best_feature_h_idx[i] = idx;
                feature_per_hyp_count[idx]++;
                summed_residuals_per_h[idx] += residualMatrix(i,idx);
//                      if (idx >= 0 && feature_per_hyp_count[idx] > 2.1){
//                              cv::circle(vis_image, corner_trajectories_filtered_and_transposed[i].back(), 9, cv::Scalar( colormap[3*idx], colormap[3*idx+1], colormap[3*idx+2],0), 5);
//                      }else{
//                              cv::circle(vis_image, corner_trajectories_filtered_and_transposed[i].back(), 9, cv::Scalar(  0 , 0, 0, 0), 5, 1);
//                      }
//
//
        }

        vis_image = vis_image_orig.clone();
        for (int i = 0; i < corner_trajectories_filtered_and_transposed.size(); i++){
                int idx  = best_feature_h_idx[i];
                if (idx >= 0 && feature_per_hyp_count[idx] > 5.1){
                        cv::circle(vis_image, corner_trajectories_filtered_and_transposed[i].back(), 9, cv::Scalar( colormap[3*idx], colormap[3*idx+1], colormap[3*idx+2],0), 5);
                }else{ //stupid feature
                        best_feature_h_idx[i] = -1;
                }
        }


        cout<<"num feature_per_hyp_count  \n"<<feature_per_hyp_count<<endl;
        summed_residuals.array() /= feature_per_hyp_count.array();

        cout<<"summed_residuals \n"<<summed_residuals<<endl;

        cout<<"summed_residuals_per_h \n"<<summed_residuals_per_h<<endl;

        summed_residuals_per_h.array() /= feature_per_hyp_count.array();
        cout<<"summed_residuals_per_h \n"<<summed_residuals_per_h<<endl;

        cout<<"total average residual error "<<summed_residuals_per_h.sum()<<endl;

        feature_assocoation = best_feature_h_idx;
        float outlier_penalty =  num_outliers * 100.0;
        printf("outlier num %d penalty %f\n", num_outliers, outlier_penalty);
        return summed_residuals_per_h.sum() ;


}

unsigned char FeatureTracker::colormap[36] =
{
                255,0,0,
                0,255,0,
                0,0,255,
                255,255,0,
                255,0,255,
                0,255,255,
                127,0,0,
                0,127,0,
                0,0,127,
                127,127,0,
                127,0,127,
                0,127,127,
};

//unsigned char FeatureTracker::colormap[768] =
//  { 150, 150, 150,
//    107, 0, 12,
//    106, 0, 18,
//    105, 0, 24,
//    103, 0, 30,
//    102, 0, 36,
//    101, 0, 42,
//    99, 0, 48,
//    98, 0, 54,
//    97, 0, 60,
//    96, 0, 66,
//    94, 0, 72,
//    93, 0, 78,
//    92, 0, 84,
//    91, 0, 90,
//    89, 0, 96,
//    88, 0, 102,
//    87, 0, 108,
//    85, 0, 114,
//    84, 0, 120,
//    83, 0, 126,
//    82, 0, 131,
//    80, 0, 137,
//    79, 0, 143,
//    78, 0, 149,
//    77, 0, 155,
//    75, 0, 161,
//    74, 0, 167,
//    73, 0, 173,
//    71, 0, 179,
//    70, 0, 185,
//    69, 0, 191,
//    68, 0, 197,
//    66, 0, 203,
//    65, 0, 209,
//    64, 0, 215,
//    62, 0, 221,
//    61, 0, 227,
//    60, 0, 233,
//    59, 0, 239,
//    57, 0, 245,
//    56, 0, 251,
//    55, 0, 255,
//    54, 0, 255,
//    52, 0, 255,
//    51, 0, 255,
//    50, 0, 255,
//    48, 0, 255,
//    47, 0, 255,
//    46, 0, 255,
//    45, 0, 255,
//    43, 0, 255,
//    42, 0, 255,
//    41, 0, 255,
//    40, 0, 255,
//    38, 0, 255,
//    37, 0, 255,
//    36, 0, 255,
//    34, 0, 255,
//    33, 0, 255,
//    32, 0, 255,
//    31, 0, 255,
//    29, 0, 255,
//    28, 0, 255,
//    27, 0, 255,
//    26, 0, 255,
//    24, 0, 255,
//    23, 0, 255,
//    22, 0, 255,
//    20, 0, 255,
//    19, 0, 255,
//    18, 0, 255,
//    17, 0, 255,
//    15, 0, 255,
//    14, 0, 255,
//    13, 0, 255,
//    11, 0, 255,
//    10, 0, 255,
//    9, 0, 255,
//    8, 0, 255,
//    6, 0, 255,
//    5, 0, 255,
//    4, 0, 255,
//    3, 0, 255,
//    1, 0, 255,
//    0, 4, 255,
//    0, 10, 255,
//    0, 16, 255,
//    0, 22, 255,
//    0, 28, 255,
//    0, 34, 255,
//    0, 40, 255,
//    0, 46, 255,
//    0, 52, 255,
//    0, 58, 255,
//    0, 64, 255,
//    0, 70, 255,
//    0, 76, 255,
//    0, 82, 255,
//    0, 88, 255,
//    0, 94, 255,
//    0, 100, 255,
//    0, 106, 255,
//    0, 112, 255,
//    0, 118, 255,
//    0, 124, 255,
//    0, 129, 255,
//    0, 135, 255,
//    0, 141, 255,
//    0, 147, 255,
//    0, 153, 255,
//    0, 159, 255,
//    0, 165, 255,
//    0, 171, 255,
//    0, 177, 255,
//    0, 183, 255,
//    0, 189, 255,
//    0, 195, 255,
//    0, 201, 255,
//    0, 207, 255,
//    0, 213, 255,
//    0, 219, 255,
//    0, 225, 255,
//    0, 231, 255,
//    0, 237, 255,
//    0, 243, 255,
//    0, 249, 255,
//    0, 255, 255,
//    0, 255, 249,
//    0, 255, 243,
//    0, 255, 237,
//    0, 255, 231,
//    0, 255, 225,
//    0, 255, 219,
//    0, 255, 213,
//    0, 255, 207,
//    0, 255, 201,
//    0, 255, 195,
//    0, 255, 189,
//    0, 255, 183,
//    0, 255, 177,
//    0, 255, 171,
//    0, 255, 165,
//    0, 255, 159,
//    0, 255, 153,
//    0, 255, 147,
//    0, 255, 141,
//    0, 255, 135,
//    0, 255, 129,
//    0, 255, 124,
//    0, 255, 118,
//    0, 255, 112,
//    0, 255, 106,
//    0, 255, 100,
//    0, 255, 94,
//    0, 255, 88,
//    0, 255, 82,
//    0, 255, 76,
//    0, 255, 70,
//    0, 255, 64,
//    0, 255, 58,
//    0, 255, 52,
//    0, 255, 46,
//    0, 255, 40,
//    0, 255, 34,
//    0, 255, 28,
//    0, 255, 22,
//    0, 255, 16,
//    0, 255, 10,
//    0, 255, 4,
//    2, 255, 0,
//    8, 255, 0,
//    14, 255, 0,
//    20, 255, 0,
//    26, 255, 0,
//    32, 255, 0,
//    38, 255, 0,
//    44, 255, 0,
//    50, 255, 0,
//    56, 255, 0,
//    62, 255, 0,
//    68, 255, 0,
//    74, 255, 0,
//    80, 255, 0,
//    86, 255, 0,
//    92, 255, 0,
//    98, 255, 0,
//    104, 255, 0,
//    110, 255, 0,
//    116, 255, 0,
//    122, 255, 0,
//    128, 255, 0,
//    133, 255, 0,
//    139, 255, 0,
//    145, 255, 0,
//    151, 255, 0,
//    157, 255, 0,
//    163, 255, 0,
//    169, 255, 0,
//    175, 255, 0,
//    181, 255, 0,
//    187, 255, 0,
//    193, 255, 0,
//    199, 255, 0,
//    205, 255, 0,
//    211, 255, 0,
//    217, 255, 0,
//    223, 255, 0,
//    229, 255, 0,
//    235, 255, 0,
//    241, 255, 0,
//    247, 255, 0,
//    253, 255, 0,
//    255, 251, 0,
//    255, 245, 0,
//    255, 239, 0,
//    255, 233, 0,
//    255, 227, 0,
//    255, 221, 0,
//    255, 215, 0,
//    255, 209, 0,
//    255, 203, 0,
//    255, 197, 0,
//    255, 191, 0,
//    255, 185, 0,
//    255, 179, 0,
//    255, 173, 0,
//    255, 167, 0,
//    255, 161, 0,
//    255, 155, 0,
//    255, 149, 0,
//    255, 143, 0,
//    255, 137, 0,
//    255, 131, 0,
//    255, 126, 0,
//    255, 120, 0,
//    255, 114, 0,
//    255, 108, 0,
//    255, 102, 0,
//    255, 96, 0,
//    255, 90, 0,
//    255, 84, 0,
//    255, 78, 0,
//    255, 72, 0,
//    255, 66, 0,
//    255, 60, 0,
//    255, 54, 0,
//    255, 48, 0,
//    255, 42, 0,
//    255, 36, 0,
//    255, 30, 0,
//    255, 24, 0,
//    255, 18, 0,
//    255, 12, 0,
//    255,  6, 0,
//    255,  0, 0,
//  };

//
//      IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp;
//
//      int win_size = 10;
//      const int MAX_COUNT = 500;
//      CvPoint2D32f* points[2] = {0,0}, *swap_points;
//      char* status = 0;
//      int count1 = 0;
//      int need_to_init = 0;
//      int night_mode = 0;
//      int flags = 0;
//      int add_remove_pt = 0;
//      CvPoint pt;
//



#if NO_ROS
int main( int argc, char** argv )
        {
      if (argc == 1)
        {
          std::cerr << "Usage: " << argv[0] << " <images>" << std::endl;
          exit(0);
        }
                cv::namedWindow("tracker", 0);
                cv::namedWindow("tracker_temp", 0);

                int num_images = argc - 1;
                int num_images_loaded = 0;
                IplImage** all_images = new IplImage*[num_images];
//              for (int i = 0 ; i < num_images; i++){
//                      all_images[i] = cvLoadImage(argv[i+1]);
//              }

                FeatureTracker ft;
                all_images[0] = cvLoadImage(argv[1]);
                num_images_loaded++;
                cv::Mat img(all_images[0]);
                ft.findCorners(img);

                cv::imshow("tracker", ft.vis_image);
                cv::waitKey();

                int c;
                for (int i = 1 ; i < num_images; i++){
                        all_images[i] = cvLoadImage(argv[i+1]);
                        num_images_loaded++;
                        printf("loading image %s\n", argv[i+1]);
                        cv::Mat img(all_images[i]);
                        ft.tracking_step(img);

                        if( (i-1) % 5 == 0){
                                Timer t;
                                ft.evluateTrajectories();
                                printf("took %f secs\n", t.stop());
                                cv::imshow("tracker", ft.vis_image);
                                c = cvWaitKey();
                                if( (char)c == 27 )
                                        break;
                        }

                }

                for (int i = 0 ; i < num_images_loaded; i++){
                        cvReleaseImage(&all_images[i]);
                }
                delete [] all_images;


        }

#else

/***
 * Main method to be run with PR2 robot
 */

int main( int argc, char** argv )
        {

                ros::init(argc, argv, "image_tracker");
                ros::NodeHandle nh;
                sensor_msgs::CvBridge _bridge;
                cv::namedWindow("tracker", 0);
                cv::namedWindow("tracker_temp", 0);


                std::string input_image_topic =
                argc == 2 ? argv[1] : "/camera/rgb/image_color";

                FeatureTracker ft;

                sensor_msgs::ImageConstPtr imgmasg_ptr = ros::topic::waitForMessage<sensor_msgs::Image>("prosilica/image_rect_color", ros::Duration(5.0));
                IplImage* ipl_img = _bridge.imgMsgToCv(imgmasg_ptr, "passthrough");
                cv::Mat img(ipl_img);

                ros::ServiceClient svc = nh.serviceClient<camera_self_filter::mask>("self_mask");
                camera_self_filter::mask servicecall;
                servicecall.request.header.frame_id = "high_def_optical_frame";
                servicecall.request.header.stamp = ros::Time::now();
                svc.call(servicecall);
                sensor_msgs::ImageConstPtr maskptr = boost::make_shared<sensor_msgs::Image>(boost::ref(servicecall.response.mask_image));

                IplImage* ipl_mask = _bridge.imgMsgToCv(maskptr, "passthrough");
                cvDilate(ipl_mask, ipl_mask, 0, 3);
                cvNot(ipl_mask,ipl_mask);
                cv::Mat mask(ipl_mask);
                
                cv::Mat ROI_mat = cv::Mat::zeros(img.rows, img.cols, CV_8U);
                cv::rectangle(ROI_mat, cv::Point(0,0), cv::Point(img.cols-1, 1400),cv::Scalar(255,255,255,0), -1);

                cv::resize(mask, mask, cv::Size(img.cols, img.rows), 0, 0, CV_INTER_NN);

                cv::bitwise_and(ROI_mat,mask, mask);


                cv::imshow("tracker", mask);
                cv::waitKey();

//              ft.findCorners(img);
                ft.findCorners(img, &mask);

                cv::imshow("tracker", ft.vis_image);
                cv::waitKey();

                int c;
                for ( ; ; ){

                        sensor_msgs::ImageConstPtr imgmasg_ptr = ros::topic::waitForMessage<sensor_msgs::Image>("prosilica/image_rect_color", ros::Duration(5.0));
                        IplImage* ipl_img = _bridge.imgMsgToCv(imgmasg_ptr, "passthrough");
                        cv::Mat img(ipl_img);
                        Timer t;
                        ft.tracking_step(img);
                        ft.evluateTrajectories();
                        printf("took %f secs\n", t.stop());
                        cv::imshow("tracker", ft.vis_image);
                        c = cvWaitKey();
                        if( (char)c == 27 )
                                break;

                }

                return 0;

        }


#endif

---

pr2_interactive_segmentation
Author(s): Dejan Pangercic, Christian Bersch, Sarah Osentoski
autogenerated on Mon Mar 4 11:14:24 2013