視覚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]
* .
* .
* .
* */