thermalvis: thermalvis: reconstruction.hpp Source File

00001 
00005 #ifndef _RECONSTRUCTION_H_
00006 #define _RECONSTRUCTION_H_
00007 
00008 #include "general_resources.hpp"
00009 #include "opencv_resources.hpp"
00010 #include "camera.hpp"
00011 #include "geometry.hpp"
00012 #include "tracks.hpp"
00013 
00014 #include <pcl/ros/conversions.h>
00015 #include <pcl/point_cloud.h>
00016 #include <pcl/point_types.h>
00017 //#include <pcl_ros/point_cloud.h>
00018 //#include <pcl_ros/publisher.h>
00019 // #include <point_cloud_redesign/publisher.h>
00020 //#include <pcl/ros/publisher.h>
00021 //#include <pcl/ros/publisher.h>
00022 //#include <pcl/ros/subscriber.h>
00023 #include <pcl/io/pcd_io.h>
00024 #include <pcl/point_types.h>
00025 
00026 #include <visualization_msgs/MarkerArray.h>
00027 
00028 // SBA Includes
00029 #include <sba/sba.h>
00030 #include <sba/sba_file_io.h>
00031 #include <sba/visualization.h>
00032 
00033 #define _USE_MATH_DEFINES
00034 #include <math.h>
00035 
00036 using namespace sba;
00037 
00038 // #include <pcl/visualization/cloud_viewer.h>
00039 
00040 #define POSITION_LIMIT                          1000
00041 
00042 
00043 #define EPSILON 0.00001
00044 // #define __SFM__DEBUG__
00045 
00046 
00047 void getTranslationBetweenCameras(Mat& C1, Mat& C2, double *translations);
00048 
00049 void triangulateTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index);
00050 
00051 unsigned int putativelyTriangulateNewTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index);
00052 unsigned int addNewPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index);
00053 
00054 void findTriangulatableTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, unsigned int latest_index, unsigned int min_length = 10);
00055 
00056 
00057 void findP1Matrix(Mat& P1, const Mat& R, const Mat& t);
00058 
00059 void reduceVectorsToTrackedPoints(const vector<Point2f>& points1, vector<Point2f>& trackedPoints1, const vector<Point2f>& points2, vector<Point2f>& trackedPoints2, vector<uchar>& statusVec);
00060 
00061 int TriangulateNewTracks(vector<featureTrack>& trackVector, const int index1, const int index2, const Mat& K, const Mat& K_inv, const Mat& P0, const Mat& P1, bool initializeOnFocalPlane = false);
00062 void ExtractPointCloud(vector<Point3d>& cloud, vector<featureTrack>& trackVector);
00063 void Triangulate(const Point2f& pt1, const Point2f& pt2, const Mat& K, const Mat& Kinv, const Mat& P1, const Mat& P2, Point3d& xyzPoint, bool debug = false);
00064 void LinearLSTriangulation(Mat& dst, const Point3d& u, const Mat& P, const Point3d& u1, const Mat& P1);
00065 void IterativeLinearLSTriangulation(Mat& dst, const Point3d& u, const Mat&  P, const Point3d& u1, const Mat&  P1);
00066 void TriangulatePoints(const vector<Point2f>& pt_set1,
00067                        const vector<Point2f>& pt_set2,
00068                        const Mat& K,
00069                        const Mat& Kinv,
00070                        const Mat& P,
00071                        const Mat& P1,
00072                        vector<Point3d>& pointcloud,
00073                        vector<Point2f>& correspImg1Pt);
00074                        
00075 void initializeP0(Mat& P);
00076 void getWandZ(Mat& W, Mat& Winv, Mat& Z);
00077 
00078 bool findBestReconstruction(const Mat& P0, Mat& P1, Mat& R, Mat& t, const SVD& svd, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2, bool useDefault = false);
00079 
00080 int addToTracks(SysSBA& sys, int im1, vector<Point2f>& pts1, int im2, vector<Point2f>& pts2);
00081 
00082 void addFixedCamera(SysSBA& sys, cameraParameters& cameraData, const Mat& C);
00083 void addNewCamera(SysSBA& sys, cameraParameters& cameraData, const Mat& C);
00084 void updateCameraNode_2(SysSBA& sys, const Mat& R, const Mat& t, int image_index);
00085 void updateCameraNode_2(SysSBA& sys, const Mat& C, int image_index);
00086 void addBlankCamera(SysSBA& sys, cameraParameters& cameraData, bool isFixed = false);
00087 
00088 void retrieveCamera(Mat& camera, const SysSBA& sys, unsigned int idx);
00089 void retrieveAllCameras(Mat *allCameraPoses, const SysSBA& sys);
00090 void retrieveAllPoints(vector<Point3d>& pts, const SysSBA& sys);
00091 void updateTracks(vector<featureTrack>& trackVector, const SysSBA& sys);
00092 
00093 void updateCameraNode(SysSBA& sys, Mat R, Mat t, int img1, int img2);
00094 void printSystemSummary(SysSBA& sys);
00095 
00096 void matrixToQuaternion(const Mat& mat, Quaterniond& quat);
00097 void quaternionToMatrix(const Quaterniond& quat, Mat& mat);
00098 
00099 float ReciprocalSqrt( float x );
00100 
00101 void transformationToProjection(const Mat& trans, Mat& proj);
00102 void projectionToTransformation(const Mat& proj, Mat& trans);
00103 
00104 void projectionToRotation(const Mat& src, Mat& dst);
00105 void rotationToProjection(const Mat& src, Mat& dst);
00106 
00107 // maxIndex says what's the latest frame for which tracks should be included
00108 void assignTracksToSBA(SysSBA& sys, vector<featureTrack>& trackVector, int maxIndex);
00109 
00110 void transfer3dPoint(const Point3d& src, Point3d& dst, const Mat& C);
00111 
00112 void transfer3DPoints(const vector<Point3d>& src, vector<Point3d>& dst, const Mat& C);
00113 
00114 void addNewPoints(SysSBA& sys, const vector<Point3d>& pc);
00115 
00116 double getRotationInDegrees(const Mat& R);
00117 double getDistanceInUnits(const Mat& t);
00118 
00119 double getQuaternionAngle(const Quaterniond& q1, const Quaterniond& q2);
00120 
00121 void convertPoint3dToMat(const Point3d& src, Mat& dst);
00122 
00123 Quaterniond defaultQuaternion();
00124 
00125 void convertProjectionMatCVToEigen(const Mat& mat, Eigen::Matrix< double, 3, 4 > m);
00126 void convertProjectionMatEigenToCV(const Eigen::Matrix< double, 3, 4 > m, Mat& mat);
00127 
00128 double dotProduct(const Mat& vec1, const Mat& vec2);
00129 double dotProduct(const Quaterniond& q1, const Quaterniond& q2);
00130 
00131 void constrainDodgyPoints(SysSBA& sys);
00132 
00133 //void convertFromMatToQuaternion(const Mat& mat, Quaterniond& quat);
00134 
00135 void convertVec4dToMat(const Vector4d& vec4, Mat& mat);
00136 
00137 void findFourTransformations(Mat *C, const Mat& E, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2);
00138 
00139 // I think these are known as a cheirality test?
00140 int pointsInFront(const Mat& C1, const Mat& C2, const vector<Point3d>& pts);
00141 int pointsInFront(const Mat& C1, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2);
00142 
00143 void compileTransform(Mat& c, const Mat& R, const Mat& t);
00144 void combineTransforms(Mat& CN, const Mat& C0, const Mat& C1);
00145 void decomposeTransform(const Mat& c, Mat& R, Mat& t);
00146 
00147 double calcFrameScore(double geomScore, int numFeatures, int numTracks);
00148 
00149 //double calcGeometryScore(vector<Point2f>& points1, vector<Point2f>& points2, Mat& F, Mat& Fmask, Mat& H, Mat& Hmask);
00150 double calcGeometryScore(int numInliers_H, int numInliers_F, double sampsonError_H, double sampsonError_F);
00151 
00152 double calcSampsonError(vector<Point2f>& points1, vector<Point2f>& points2, Mat& H, Mat& Hmask);
00153 
00154 double calcSampsonDistance(Point2f& pt1, Point2f& pt2, Mat& F);
00155 
00156 double calcInlierGeometryDistance(vector<Point2f>& points1, vector<Point2f>& points2, Mat& mat, Mat& mask, int distMethod = SAMPSON_DISTANCE);
00157 
00158 void assignIntrinsicsToP0(Mat& P0, const Mat& K);
00159 
00160 // I think this will give you all of the points that were successfully tracked from idx1 to idx2
00161 void getPointsFromTracks(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, int idx1, int idx2);
00162 void getPoints3dFromTracks(vector<featureTrack>& tracks, vector<Point3d>& cloud, int idx1, int idx2);
00163 
00164 // Gets the 2d locations of points that have been tracked
00165 void getCorrespondingPoints(vector<featureTrack>& tracks, const vector<Point2f>& pts1, vector<Point2f>& pts2, int idx0, int idx1);
00166 
00167 // Given some 3D co-ordinates and two image indices, find the corresponding image points that show as many of 
00168 // these as possible (actually it should be all of them if this is an intermediate point)
00169 void findFeaturesForPoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, vector<Point3d>& pts3d, int idx1, int idx2);
00170 
00171 void retrieveCameraPose(SysSBA& sys, Mat& camPose, int idx);
00172 
00173 void estimateNewPose(vector<featureTrack>& tracks, Mat& K, int idx, Mat& pose);
00174 
00175 void obtainAppropriateBaseTransformation(Mat& C0, vector<featureTrack>& tracks);
00176 
00177 void findCentroidAndSpread(vector<featureTrack>& tracks, Point3d& centroid, Point3d& deviations);
00178 
00179 void filterToCompleteTracks(vector<unsigned int>& dst, vector<unsigned int>& src, vector<featureTrack>& tracks, int idx1, int idx2);
00180 void getActiveTracks(vector<unsigned int>& indices, vector<featureTrack>& tracks, int idx1, int idx2);
00181 
00182 int countActiveTriangulatedTracks(vector<unsigned int>& indices, vector<featureTrack>& tracks);
00183 
00184 void updateTriangulatedPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<Point3d>& cloud);
00185 
00186 void getActive3dPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<Point3d>& cloud);
00187 
00188 // Gets 2D locations of active tracks into simple vectors
00189 void filterToActivePoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, vector<unsigned int>& indices, int idx1, int idx2);
00190 
00191 void reduceActiveToTriangulated(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<unsigned int>& untriangulated);
00192 
00193 void updateSystemTracks(SysSBA& sys, vector<featureTrack>& tracks, unsigned int start_index);
00194 
00195 int findBestCandidate(const Mat *CX, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2, Mat& C);
00196 
00197 void summarizeTransformation(const Mat& C, char *summary);
00198 
00199 void reverseTranslation(Mat& C);
00200 
00201 void removeShortTracks(vector<featureTrack>& tracks, int idx1, int idx2);
00202 
00203 bool pointIsInFront(const Mat& C, const Point3d& pt);
00204 
00205 void getIndicesForTriangulation(vector<unsigned int>& dst, vector<unsigned int>& src, vector<unsigned int>& already_triangulated);
00206 
00207 void reconstructSubsequence(vector<featureTrack>& tracks, vector<Point3d>& ptCloud, int idx1, int idx2);
00208 
00209 
00210 
00211 void findCentroid(vector<featureTrack>& tracks, Point3d& centroid, Point3d& stdDeviation);
00212 
00213 // Between two images, gets all 2D point locations and 3D locations needed for BA
00214 void getTriangulatedFullSpanPoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, int idx1, int idx2, vector<Point3f>& points3);
00215 
00216 bool estimatePoseFromKnownPoints(Mat& dst, cameraParameters camData, vector<featureTrack>& tracks, unsigned int index, const Mat& guide);
00217 
00218 #endif