功能：建立執行緒

boost::thread

舉例說明：

#include <boost/thread/thread.hpp>
#include <iostream>

void hello()
{
        std::cout <<
        "Hello world, I'm a thread!"
        << std::endl;
}

int main(int argc, char* argv[])
{
        boost::thread thrd(&hello);//這裡用一個回撥函式函式
        thrd.join();//這裡讓一個執行緒開始
 

        return 0;
}

功能：互斥體

boost::mutex

任何寫過多執行緒的人都知道，避免不同執行緒同時訪問共享區域的重要性。如果一個執行緒想要更改他共享區域中的某個資料，這時，另一個執行緒正在讀取這個資料。在這個時候就要用到互斥體。一個互斥體只允許一個執行緒訪問共享區。當另一個執行緒想要訪問共享區的時候，首先要做的就是鎖住（lock）
參考部落格：http://www.cnblogs.com/chengmin/archive/2011/12/29/2306416.html

建構函式初始化列表，
引數說明：

從地圖到世界座標
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0
 
, 0, 0 ), tf::Point(0, 0, 0 )))從RPY中創造四元數
 laser_count_(0)，鐳射雷達計數
 private_nh_("~"),私有的控制代碼
scan_filter_sub_(NULL)，鐳射雷達訂閱
scan_filter_(NULL),鐳射雷達濾波器
transform_thread_(NULL)，變換的執行緒，

   unsigned long int seed_;這是一個長整型

目前是用roboware進行跳轉，真的很easy。

這裡建構函式三次過載。
在定義的時候，用一個*，然後將來例項化的就是&

gmapping這個名稱空間是在basic GridFastSLAM演算法的基礎上，每個粒子都有其自己的地圖和機器人的位姿。

第一個是初始化預設引數
第二個是預設的輸出流 infoStr是輸出流
所以，這裡的是為了初始化引數

然後跳轉到這裡，是傳遞的引數

讓其里程計的資料為NULL，空指標

bool型別的採用false
然後進行實時的傳值

throttle_scans表示處理的容忍度吧，最小的容忍度。如果設定很高，就會漏掉更多的點

設定引數的名字，已經數字，和string型別，都可以，然後對這個引數進行設定

以及這裡的數字

返回的是一些引數

以及TF釋出的時間

map_update_interval地圖更新的間隔

這些引數都是關於gmapping的封裝wrapper
gmapping自己的引數

minmumScore最小化評分，來評價scan的資料是不是好，也就是對他的置信度，在小範圍餓鐳射雷達例如5米的情況下

srr里程計的平移的/平移
srt里程計的平移/旋轉
stt里程計的旋轉/旋轉
str里程計的旋轉/平移

NULL是巨集 false是關鍵字

lskip 每束鐳射雷達跳過的鐳射束
ogain為了重新取樣而評估的增益
lstep 平移優化的步長
astep 旋轉優化的步長
kernelSize尋找關聯的
sigma用於暴力匹配
lsigma 每束鐳射雷達掃描的可能性

void startLiveSlam:
entropy熵（平均值）

這三個引數依次是 話題的名稱，佇列的長度，和是否釋出訊息
這裡是釋出的訊息

訂閱的話題

最後開始一個執行緒

關於獲得引數的補充：

訪問私有名稱作為工作空間

開始一個迴應startReplay
輸入的的 bag的的名字 和 鐳射雷達的topic

開啟一個新的

如果不能夠處理的話，5條訊息。
std::queue是容器介面卡用來適配FIFO

std::pair<int,float>將兩個資料融合成一個數據。

這裡是例項化了一個物件，下面肯定會有s_queue.first 和 s_queue.second

這裡是一個智慧指標
foreach用來遍歷
在32位系統中的size_t是4個位元組，size_t是8個位元組 size_t一般用來表示計數，比如有多少東西拷貝
這是使用了一個定時器

示例程式：

#include "ros/ros.h"
d callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}
void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);
  ros::spin();
  return 0;
}

TimerEvent結構體提供當前定時器的時間資訊

callbacks回撥函式
單執行緒spinning

ros::spin();

在這個程式中，使用者的回撥函式在ros::spin()中被呼叫，ros::spin()在節點關閉或者在ros::shutdown();或者按下ctrl+c才會返回。
定期執行ros::spinonce();

ros::Rate r(19);//19Hz的釋出頻率
while(ros::ok())
{
    ros::spinOnce();//會呼叫所有的回撥函式
    r.sleep();
}

ros當中的多執行緒

方式1：這是一種阻塞式的

ros::MultiThreadedSpinner spinner(4);//使用了4個執行緒
spinner.spin();//spin()直到節點被關閉

方式2：這是一個非阻塞式的

ros::AsyncSpinner spinner(4);
spinner.start();
ros::waitForShutdown();

callbackqueue回撥佇列

#include<ros/callback_queue.h>
ros::CallbackQueue my_queue;

Logging(日誌)

ROS_DEBUG(...)
ROS_DEBUG_STREAM(args)

舉例

#include <ros/console.h>
ROS_DEBUG("Hello %s", "World");
ROS_DEBUG_STREAM("Hello " << "World");

cond表示 condition（條件）
當條件為真，就會輸出日誌資訊

include <ros/console.h>
ROS_DEBUG_COND(x < 0, "Uh oh, x = %d, this is bad", x);

ros當中轉換時間和持續時間

double secs =ros::Time::now().toSec();
ros::Duration d(0.5);
secs = d.toSec();

指定程式的休眠時間

ros::Duration(0.5).sleep(); // sleep for half a second

ros的釋出和訂閱

ros::Publisher pub = nh.advertise<std_msgs::String>("topic_name", 5);
std_msgs::String str;
str.data = "hello world";
pub.publish(str);

後面那個引數表示佇列的長度。


tf的資料結構：
tf::Quaternion tf::Vector3 tf::Point tf::Pose tf::Transform
tf::Stamped模板，就是一個模板類 附帶元素 frame_id_和stamp_
tf::StampedTransform 是tf::Transforms的特例，它要求frame_id、stamp、child_frame_id
函式：tf::Quaternion createIdentityQuaternion()
作用：返回四元數控制代碼
函式：tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)
作用：返回從固定軸的Roll, Pitch and Yaw(滾動，俯仰和偏轉)構造的tf::Quaternion四元數
函式：geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)
作用：返回從固定軸的Roll, Pitch and Yaw(滾動，俯仰和偏轉)構造geometry_msgs::Quaternion四元數
tf::TransformBroadcaster();傳送變換。
傳送變換通過呼叫sendTransform()函式實現
canTransform()函式 返回bool ，判斷能否實現變換。不會丟擲異常，如果出錯，會返回error_msg的內容。
waitForTransform()函式 返回bool值，評估變換是否有效
異常類：tf::ConnectivityException
作用：如果由於兩個座標系ID不在同一個連線的樹中而無法完成請求，則丟擲。
異常類：tf::ExtrapolationException
作用：如果請求的座標系id之間存在連線，但一個或多個變換已過期，則丟擲。
異常類：tf::InvalidArgument
作用：如果引數無效則丟擲。 最常見的情況是非規範化的四元數。
異常類：tf::LookupException
作用：如果引用了未釋出的座標系ID，則丟擲。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

在C語言當中 atan2(double x,double y)返回的原點到(x,y)的方位角，也就是與x軸的夾角。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);
  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

增加一個carrot的座標系

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1")

同樣這各也是一個框架希望自己在使用的時候能夠記住

  ros::Rate rate(10.0);
  while(ros::ok())
  {
        rate.sleep();
}

ros::Time(0);  說明察覺到這個時間之後立即行動。遲疑的時間是0秒。

同樣的意思

ros::Time::now();

ros::Duration(3.0)；持續時間是3秒

actionlib裡面很多內容暫時跳過去，因為在gmapping裡面看到的比較少。

pluginlib外掛的介紹
在package.xml檔案中包含export項
這裡寫學的不是很好

nodelet的用途和使用
nodelet包用來設計在演算法之間的具有零拷貝傳輸相同的程序中執行多個演算法
基本的用法：

nodelet usage:
nodelet load pkg/Type manager - launch一個nodelet的管理器
nodelet standalone pkg/Type   - 匯入一個單個節點 node
nodelet unload name manager   - 從manager匯入輸出一個單個的節點
nodelet manager               - 開始manager的節點

可以在launch檔案中

匯入一些節點。

同時我們可以開發屬於自己的基控制器然後把這個事情做好。
我覺得這個nodelet的地方，到時候需要看看一下
然後這個下面還有一些實現路徑規劃功能的包。

言歸正傳，我們接著看gmapping的原始碼
找到一個不錯的看原始碼的工具scientific toolworks understand，然後安裝連線啥的，大家自行百度，剛剛沒存檔，就給沒了，哈哈。
其實我覺著就是一個很好的影象化顯示的東西，能夠讓你很直觀的看到
然你旋轉用ctrol flow的時候，一定要注意這點著函式然後走，我嘗試過，這個軟體不能看ros的原始碼，只能在幾個標頭檔案當中相互跳轉，但是實際用的時候，還是可以用roboware跳轉來看ros的原始碼

就是先從這個laserCallback函式，然後中間嵌套了

void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
  if ((laser_count_ % throttle_scans_) != 0)
    return;

  static ros::Time last_map_update(0,0);

  // We can't initialize the mapper until we've got the first scan
  if(!got_first_scan_)
  {
    if(!initMapper(*scan))
      return;
    got_first_scan_ = true;
  }

  GMapping::OrientedPoint odom_pose;

  if(addScan(*scan, odom_pose))
  {
    ROS_DEBUG("scan processed");

    GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
    ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
    ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);

    tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));

    map_to_odom_mutex_.lock();
    map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
    map_to_odom_mutex_.unlock();

    if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
    {
      updateMap(*scan);
      last_map_update = scan->header.stamp;
      ROS_DEBUG("Updated the map");
    }
  } else
    ROS_DEBUG("cannot process scan");
}

首先給鐳射雷達的極速韓，然後初始化地圖。

bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
  laser_frame_ = scan.header.frame_id;
  tf::Stamped<tf::Pose> ident;  設定單位矩陣
  tf::Stamped<tf::Transform> laser_pose; 這是他的鐳射雷達的位姿
  ident.setIdentity(); 設定單位矩陣
  ident.frame_id_ = laser_frame_; 這是frameid
  ident.stamp_ = scan.header.stamp; 這是ID
  try  這裡用來丟擲一個異常，其實就是檢視著兩個TF之間的關係
  {
    tf_.transformPose(base_frame_, ident, laser_pose);兩個TF之間的關係
  }
  catch(tf::TransformException e)這裡輸出一個e
  {
    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
             e.what());
    return false;
  }
創造一個點在鐳射雷達laser_pose是他的位姿。然後就是在原點
  // create a point 1m above the laser position and transform it into the laser-frame
  tf::Vector3 v;  其實鐳射雷達的位姿，然後
  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
                                      base_frame_);
  try
  {
    tf_.transformPoint(laser_frame_, up, up);
    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
  }
  catch(tf::TransformException& e)
  {
    ROS_WARN("Unable to determine orientation of laser: %s", 這裡就是丟擲一個異常
             e.what());
    return false;
  }

  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
  if (fabs(fabs(up.z()) - 1) > 0.001)
  {
    ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
                 up.z());
    return false;
  }

  gsp_laser_beam_count_ = scan.ranges.size();對gmapping 進行計算數值

  double angle_center = (scan.angle_min + scan.angle_max)/2;得到中間的角度值。

  if (up.z() > 0)
  {
    do_reverse_range_ = scan.angle_min > scan.angle_max;如果最小的值比最大的值大的話，那麼返回一個bool值
    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);得到中心值，鐳射雷達啊的TF。然後這個裡面實現的過就是通尤拉角轉換輸出的四元數，然後設定3維的限量，最後輸出的laser_frame，執行時間的話是立即生效。
    ROS_INFO("Laser is mounted upwards.");
  }
  else
  {
    do_reverse_range_ = scan.angle_min < scan.angle_max;
    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
    ROS_INFO("Laser is mounted upside down.");
  }

  // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
  laser_angles_.resize(scan.ranges.size());
  // Make sure angles are started so that they are centered
  double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
  for(unsigned int i=0; i<scan.ranges.size(); ++i)
  {
    laser_angles_[i]=theta;
    theta += std::fabs(scan.angle_increment);
  }

  ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
            scan.angle_increment);
  ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
            laser_angles_.back(), std::fabs(scan.angle_increment));

  GMapping::OrientedPoint gmap_pose(0, 0, 0);

  // setting maxRange and maxUrange here so we can set a reasonable default
  ros::NodeHandle private_nh_("~");
  if(!private_nh_.getParam("maxRange", maxRange_))
    maxRange_ = scan.range_max - 0.01;
  if(!private_nh_.getParam("maxUrange", maxUrange_))
    maxUrange_ = maxRange_;

  // The laser must be called "FLASER".
  // We pass in the absolute value of the computed angle increment, on the
  // assumption that GMapping requires a positive angle increment.  If the
  // actual increment is negative, we'll swap the order of ranges before
  // feeding each scan to GMapping.
  gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                         gsp_laser_beam_count_,
                                         fabs(scan.angle_increment),
                                         gmap_pose,
                                         0.0,
                                         maxRange_);
  ROS_ASSERT(gsp_laser_);

  GMapping::SensorMap smap;
  smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
  gsp_->setSensorMap(smap);

  gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
  ROS_ASSERT(gsp_odom_);


  /// @todo Expose setting an initial pose
  GMapping::OrientedPoint initialPose;
  if(!getOdomPose(initialPose, scan.header.stamp))
  {
    ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
    initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
  }

  gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
                              kernelSize_, lstep_, astep_, iterations_,
                              lsigma_, ogain_, lskip_);

  gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
  gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
  gsp_->setUpdatePeriod(temporalUpdate_);
  gsp_->setgenerateMap(false);
  gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                                delta_, initialPose);
  gsp_->setllsamplerange(llsamplerange_);
  gsp_->setllsamplestep(llsamplestep_);
  /// @todo Check these calls; in the gmapping gui, they use
  /// llsamplestep and llsamplerange intead of lasamplestep and
  /// lasamplerange.  It was probably a typo, but who knows.
  gsp_->setlasamplerange(lasamplerange_);
  gsp_->setlasamplestep(lasamplestep_);
  gsp_->setminimumScore(minimum_score_);

  // Call the sampling function once to set the seed.
  GMapping::sampleGaussian(1,seed_);

  ROS_INFO("Initialization complete");

  return true;
}

/tf::Stamped 對上述資料型別做模板化（除了tf::Transform），並附帶元素frame_id_和stamp_ 。
identity matrix 單位矩陣，
我們定義tf::Transform transform 我們定義設定資料型別用來存放轉換資訊的（簡單來說 平移和旋轉的向量）
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );我懂了，這個地方是設定鐳射雷達餓起始位置，這個還是不錯的

當然你也可以定義旋轉

 tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);

然後在將這個tf廣播出去

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

同樣定義一個監聽器

tf::TransformListener listener

定義存放一個關係變數

 tf::StampedTransform transform;

      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);

然後通過這個函式lookupTransform用來監聽兩個TF之間的關係。
設定鐳射的角度，

通過resize這種方式，傳遞引數
將所有的鐳射累的資料進行累加，

這裡就是實現的將所有的鐳射累的資料進行的累計的疊加。
然後這就是輸出鐳射雷達的一些增量。
設定最大的R（也就是半徑的範圍）以及設定最大的U的範圍
這個鐳射雷達的名字必須被叫做FLASER,我們通過一些絕對值來計算增量。gmapping需要計算一些正向的角度增量。如果這個角度增量式一個複製，我們需要交換角度的順序才能將鐳射雷達的進行填充。
這裡需要例項化一個物件

position在哪裡插入點。插入點的數量n。pt 插入的點。

  GMapping::SensorMap smap;
  smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));//insert
  gsp_->setSensorMap(smap);//將這個地圖得到地圖

make_pai

make_pair(gsp_laser_->getName(), gsp_laser_);//返回的值是一個值，就是將他們的資料結構。
然後在通過，insert將兩個資料結合起來。
gsp_->setSensorMap(smap);

然後

  void GridSlamProcessor::setSensorMap(const SensorMap& smap){     
    SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
    if (laser_it==smap.end()){
      cerr << "Attempting to load the new carmen log format" << endl;
      laser_it=smap.find(std::string("ROBOTLASER1"));
      assert(laser_it!=smap.end());
    }
    const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
    assert(rangeSensor && rangeSensor->beams().size());

    m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
    double* angles=new double[rangeSensor->beams().size()];
    for (unsigned int i=0; i<m_beams; i++){
      angles[i]=rangeSensor->beams()[i].pose.theta;
    }
    m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
    delete [] angles;
  }

.find(std::string("FLASER")全域性尋找FLASER標誌

然後將角度進行賦值。，對著將這裡匹配到的地圖資料，釋出到鐳射雷達的引數當中。

他的輸入就是一個map的資料型別。
然後例項化一個里程計的型別。然後里程計資訊進行賦值。
下面我們打算設定初始化的位姿。
首先初始化一個位姿。，
這個bool型別，然後在bool型別當中，進行初始化很資料型別

stamp表示的時間戳吧。centered_laser_pose_.stamp_ = t;得到鐳射雷達的位姿。佳作odom_pose

tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);

初始化一些他們之間的位姿關係。得到yaw ，然後將這個值，給了傳入的引數的數值。

  gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
                                      odom_pose.getOrigin().y(),
                                      yaw);

然後做出了一些判斷，就是判斷，其實就是判斷有沒有得到位姿。
然後進行匹配一些引數

第一步就是匹配鐳射雷達的引數，然後設定運動模型的引數，第三步，更新距離的引數，第四步，設定更新的時間，第五步 設定更新的地圖。 第六步，進行初始化粒子的大小，第七步，設定取樣的範圍。第八步，設定鐳射雷達的步長 第九步，設定最小化得分。
第一步：
設定鐳射雷達的引數。
第二步：

設定運動模型，本質上還是傳遞引數。
第三步：
同樣將傳遞的引數進行賦值。

第四步：同樣傳遞引數。
第五步：設定丟他
回到lasercallback執行緒當中，然後將scan和odom_pose結合起來。一般bool型別，就是用來if語句來進行判斷。首先我得到getOdompose函式，這是一種bool型別。將num_ranges理邏輯。就是這兩個for型別。然後讀取reading函式，將ranges_double，然後設定位姿 其實就是一個傳參。將reading這個數值，給的資料傳遞給processscan
這裡返回的是一個bool型別。這個bool型別。bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
從reading當中，獲得位姿，同時計算里程計。首先我們初始化，然後獲得位姿。將上一個幀的位姿進行傳參。使用運動模型來更新例子。初始化一個引數，畫出運動模型，

OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
    double sxy=0.3*srr;
    OrientedPoint delta=absoluteDifference(pnew, pold);
    OrientedPoint noisypoint(delta);
    noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
    noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
    noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
    noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
    if (noisypoint.theta>M_PI)
        noisypoint.theta-=2*M_PI;
    return absoluteSum(p,noisypoint);
}

就是當前時刻的位姿，前一個時刻的位姿，

計算新舊幀的絕對誤差

absoluteDifference(pnew, pold);

從這裡看的話，我覺得是進行添加了一下高斯的擾動模型

最後輸出值，我估計是P

pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);

將這些的更新的狀態給了pose
然後更新輸出的檔案，判斷是不是打開了這個檔案，然後設定了了一下精度。將當前的位置狀態的資訊，進行了了輸出。然後將一些粒子的更新進行了輸出。以及讀取的時間。
最後生成一個回撥函式，這個函式是空的。
接下來計算機器人的平移和旋轉。
計算機器人的旋轉和平移
計算角度的角度的變化 通過使用move將前一時刻機器人的位姿和當前時刻機器人的位姿進行做差。然後就是知道move.theta=atan2(sin(move.theta), cos(move.theta));就是線速度的變換和加速度的變換。
隨後進行一次將來原來的pose資訊給了當前的位置資訊。進行了一次迭代。處理了一次迭代。更新當前幀
其實本質上就是次scan-matcher，然後就是一寫賦值。如果計數是大於0的話，然後就進行的掃描的匹配。

使用這個scanMatch來匹配每一幀的粒子。

inline void GridSlamProcessor::scanMatch( 
 

            
  
           

              
              
            
            
相關推薦
			   
            
            
            
 

    

    
    
gmapping原始碼分析以及收穫
     
 
							
							
							





功能：建立執行緒



boost::thread

舉例說明：



#include <boost/thread/thread.hpp>
#include <iostream>

void hello()
{
       

  
 

    

    
    
SurfaceView原始碼分析以及使用
     
  
  
  
 概述 
 SurfaceView是Android中比較特殊的一類檢視，它與普通的View最主要的區別是它和它的檢視容器並不是在一個檢視層。 
 為什麼要使用SurfaceView 
 我們知道Android系統中是提供了View來進行繪圖處理，然後可以通過invalidate方法通知系統去呼 

  
 

    

    
    
安卓進階(3)之Handler/Looper/MessageQueue原始碼分析以及原理理解
     
 
							
							
							前言
安卓系統是訊息驅動的，所以深刻了解Handler整個訊息分發機制，對於我們瞭解安卓系統，是一個必不可少的知識點。整個過程中，我們需要重點關注的類是:
1. Handler
2. Looper
3. MessageQueue
4. Meesage
5. Th 

  
 

    

    
    
OpenCV兩種畸變校正模型原始碼分析以及CUDA實現
     
  
 
 http://www.cnblogs.com/riddick/p/7811877.html 
 影象演算法中會經常用到攝像機的畸變校正，有必要總結分析OpenCV中畸變校正方法，其中包括普通針孔相機模型和魚眼相機模型fisheye兩種畸變校正方法。 
 普通相機模型畸變校正函式針對OpenCV中的c 

  
 

    

    
    
【分享牛】Java架構群523988350（QQ群）Activiti交流群：475458061。 Activiti原始碼分析以及實戰視訊購買地址http://www.shareniu.com/
     
 
			
				分享牛
			
			Java架構群523988350（QQ群）Activiti交流群：475458061。 Activiti原始碼分析以及實戰視訊購買地址http://www.shareniu.com/...
		
		
         

  
 

    

    
    
tensorflow中RNNcell原始碼分析以及自定義RNNCell的方法
     
 
							
							
							我們在模擬一些論文的時候經常會遇到一些模型，對RNN或者LSTM進行了少許的修改，或者自己定義了一種RNN的結構等情況，比如前面介紹的幾篇memory networks的論文，往往都需要按照自己定義的方法來構造RNN網路。所以本篇部落格就主要總結一下RNNcel 

  
 

    

    
    
MapReduce階段原始碼分析以及shuffle過程詳解
     
 MapReducer工作流程圖： 
1. MapReduce階段原始碼分析 
1）客戶端提交原始碼分析 
解釋：   - 判斷是否列印日誌   - 判斷是否使用新的API，檢查連線   - 在檢查連線時，檢查輸入輸出路徑，計算切片，將jar、配置檔案複製到HDFS   - 計算切片時，計算最小切片數（預設為1 

  
 

    

    
    
Android Apk資源載入機制原始碼分析以及資源動態載入實現系列文章
     
 
							
							
							Android系統中執行Apk時是如何對包內的資源進行載入以及我們開發中設定相關資源後又是如何被加載出來，這個系列我們可以學習系統載入資源的機制原理，然後我們再巧妙的利用學習系統載入技巧來打造我們自己的動態資源載入機制實現。

這個系列主要分為如下3部分內容來講 

  
 

    

    
    
分享牛（Java架構群523988350（QQ群）Activiti交流群：475458061。 Activiti原始碼分析以及實戰視訊購買地址http://www.shareniu.com/）
     
 
            activiti工作流敏捷開發
            
              Activiti專案是一項新的基於Apache許可的開源BPM平臺，從基礎開始構建，旨在提供支援新的BPMN 2.0標準，包括支援物件管理組（OMG），面對新技術的機遇，諸如互操作性和雲架構，提供技 

  
 

    

    
    
STL list原始碼分析以及實現
     
 
							
							
							本文主要內容如下：



1. STL list實現的三個模組節點__list_node，迭代器__list_iterator以及list本身(使用一個__list_node*代表整個連結串列)的介紹。



2. 重點分析list的幾個核心函式，理解STL l 

  
 

    

    
    
Android  RemoteViews原始碼分析以及擴充套件使用
     
 
                
一，寫在前面         
       在前面兩篇文章RemoteViews的基本使用(上)之通知欄 ，RemoteViews的基本使用(下)之視窗小部件 中講述了RemoteViews的兩個應用場景，這篇文章主要介紹RemoteViews的內部機制，以及一個小擴充套 

  
 

    

    
    
ReentrantLock 原始碼分析以及 AQS （一）
     
 ## 前言

JDK1.5 之後釋出了JUC（java.util.concurrent），用於解決多執行緒併發問題。AQS 是一個特別重要的同步框架，很多同步類都藉助於 AQS 實現了對執行緒同步狀態的管理。

AQS 中最主要的就是獨佔鎖和共享鎖的獲取和釋放，以及提供了一些可中斷的獲取鎖，超時等待鎖等方法。 

  
 

    

    
    
ReentrantReadWriteLock 原始碼分析以及 AQS 共享鎖 （二）
     
 ## 前言

上一篇講解了 AQS 的獨佔鎖部分（參看：[ReentrantLock 原始碼分析以及 AQS （一）](https://mp.weixin.qq.com/s/dDjbR76U5C696CXAOAvpng)），這一篇將介紹 AQS 的共享鎖，以及基於共享鎖實現讀寫鎖分離的 ReentrantRe 

  
 

    

    
    
HashMap的學習以及原始碼分析
     
  
  
  
 Hashmap HashMap繼承AbstractMap類，實現了Map介面（由下圖可見），在java集合中，它是一個基本的儲存資料的結構。他的底層是由 陣列+連結串列 構成，通過特定的雜湊函式從 鍵（key）來定位值。 
  
 HashMap的結構形式大概如圖所示：  構造雜湊函式 
  

  
 

    

    
    
Tomcat(二) Tomcat實現： Servlet與web.xml介紹 以及 原始碼分析Tomcat實現細節
     
  
 
 轉載自;http://blog.csdn.net/tjiyu/article/details/54590259 
 
      -------如有侵權  請聯絡我 我會進行刪除
  
 
   
   
       在《Tomcat(一 

  
 

    

    
    
Spring裡的aop實現方式和原始碼分析 java中代理，靜態代理，動態代理以及spring aop代理方式，實現原理統一彙總
     
 使用"橫切"技術，AOP把軟體系統分為兩個部分：核心關注點和橫切關注點。業務處理的主要流程是核心關注點，與之關係不大的部分是橫切關注點。橫切關注點的一個特點是，他們經常發生在核心關注點的多處，而各處基本相似，比如許可權認證、日誌、事務。AOP的作用在於分離系統中的各種關注點，將核心關注點和橫切關注點分離開來。 

  
 

    

    
    
Spring原始碼分析(十五)Spring中常用註解使用以及原始碼分析
     
  
  
  
 從Java5.0開始，Java開始支援註解。Spring做為Java生態中的領軍框架，從2.5版本後也開始支援註解。相比起之前使用xml來配置Spring框架，使用註解提供了更多的控制Spring框架的方式。 
 現在越來越多的專案也都在使用註解做相關的配置，但Spring的註解非常多，相信 

  
 

    

    
    
MyBatis 本地快取和二級快取使用以及原始碼分析 第一篇
     
  
 
 本地快取 
    也稱為一級快取，分為兩個作用域SESSION和STATEMENT。官網中的描述：MyBatis利用本地快取機制（Local Cache）防止迴圈引用（迴圈引用）和加速重複巢狀查詢。預設值為SESSION，這種情況下會快取一個會話中執行的所有查詢。若設定值為ST 

  
 

    

    
    
MyBatis 本地快取和二級快取使用以及原始碼分析 第二篇
     
  
 
                                          本篇分析 

  
 

    

    
    
現代編譯原理——第四章：語義分析以及原始碼
     
 　　轉自： http://www.cnblogs.com/BlackWalnut/p/4527845.html 
　　寫完語義分析的程式碼後感覺語義分析只是為了進行型別檢測（後來才發現，這只是語義分析的一部分）。詞法分析注重的是每個單詞是否合法，以及這個單詞屬於語言中的哪些部分。語法分析的上下文無關