視覚slam 14 ch 5 joinMap.cppコードコメント(ノート版)

5918 ワード

  :https://www.cnblogs.com/newneul/p/8407369.html
#include 
#include 
using namespace std;
#include 
#include 
#include  
#include   // for formating strings
#include  
#include  
#include 

int main( int argc, char** argv )
{
    /*        5 ,        */
    vector<:mat> colorImgs, depthImgs;    //        

    /*vector     ,           ,     Eigen            ,   5         5     */
    vector<:isometry3d>> poses;   //     
    
    ifstream fin("./pose.txt");
    if (fin.bad())//             !
    {
        cerr<>d;//        fin pose.txt      d  
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //    data[6]      coeffis          
        Eigen::Isometry3d T(q);                                     //           ,
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//             
        poses.push_back( T );   //            
    }
    
    //        
    //      
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;//
    
    cout< PointCloud;
    
    //       //PointCoud::Ptr                            
    /*Ptr       ,    PointCloud   PointT pcl::PointXYZRGB  。    ->       PointCloud   
     *Ptr      boost::shared_ptr > */
    /*pointCloud                    http://blog.csdn.net/worldwindjp/article/details/18843087*/
    PointCloud::Ptr pointCloud( new PointCloud );
//    pcl::PointCloud<:pointxyzrgb>::Ptr pointCloud( new pcl::PointCloud<:pointxyzrgb> );

    /* 5                                     for     pcl           pcl       */
    for ( int i=0; i<5; i++ )//  5   
    {
        cout<(471,537)[0] = 12;//              
        //color.ptr<:vec3b>(471)[537][0] = 12;//        

        //         ,      cout<(471,537)[0]               ,           8  unsigned char
        //   ascii                ,              int  (                  ),          
        //         cout     char        ,         void *           char         。
        if(colorImgs[i].channels() == 3) {
             std::cout << "  1   " << color.ptr<:vec3b>(471)[537] << "     :  "
                       << (char) color.at<:vec3b>(471, 537)[0] << std::endl;
             std::cout << depth.ptr(471)[537] << std::endl;
             std::cout << colorImgs[i].at<:vec3b>(471, 537) << std::endl;
        }
                           
         */

        /*           ,            K            ,        T             */
        for ( int v=0; v ( v )       */
                unsigned int d = depth.ptr ( v )[u]; //    16      color           8 

                 /*                         :
                 *            16               8   
                 * 1、         for  ,      at  
                 * for ( int v=0; v(v,u);
                 *
                 *
                 * 2、            
                 *     for   
                 * cv::MatIterator_ begin,end;
                 * for( begin =depth.begin(), end = depth.end(); begin != end; ){}
                 *
                 * 3、               
                 * */

                //          ,          ,       unsigned short,     unsigned short
                //begin         
                if ( d==0 ) continue;                //  0              for                           is_dense  
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;    //          
                point[0] = (u-cx)*point[2]/fx;      //    5.5  ---86 
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;   //              
                
                PointT p ;
                p.x = pointWorld[0];    //           pcl           
                p.y = pointWorld[1];
                p.z = pointWorld[2];

                /*  color.step       ,              operator size_t() const;
                 *    color.size         size_t     ,       1920           MAT             buf[]  */
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];

                /*  ->                              Ptr      */
                pointCloud->points.push_back( p );//       
            }
    }
    std::cout<width<height<<:endl pointcloud-="">is_dense = false;
    std::cout<width<height<<:endl cout="">size()<(v,u)[0]             
 *      image.at(v,u)[1]
 *      image.at(v,u)[2]
 * 2、        (             cv::Mat    )
 * cv::MatIterator_begin,end;
 * for( begin = image.begin(), end = image.end() ; begin != end;  )
 *      (*begin)[0] = ...
 *      (*begin)[1] = ...
 *      (*begin)[2] = ...
 *
 * 3、        
 * for(v)
 *  for(u)
 *      image.ptr(v)[u][0]        
 *      image.ptr(v)[u][0]       
 *              .
 *              .
 *              .
 * */