// 第一个子图或冻结，不优化子图位姿。 也就是不优化初值 if (first_submap || frozen) { first_submap = false; // Fix the pose of the first submap or all submaps of a frozen // trajectory. problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data()); } }
const std::string& landmark_id = landmark_node.first; // 轨迹中第一个 node_data constauto& begin_of_trajectory = node_data.BeginOfTrajectory(observation.trajectory_id); // 如果 landmark observation was made before the trajectory was created if (observation.time < begin_of_trajectory->data.time) { continue; } /* 以下语句Find the trajectory nodes before and after the landmark observation */
// 找到在landmark观测时间后的第一个节点 auto next = node_data.lower_bound(observation.trajectory_id, observation.time); /* The landmark observation was made, but the next trajectory node has not been added yet. 即next已经是轨迹最后一个节点 */ if (next == node_data.EndOfTrajectory(observation.trajectory_id) ) { continue; } // 如果是刚开始的node data if (next == begin_of_trajectory) { next = std::next(next); } // 这里的pre配合next是为了获取两个位置，找到landmark观测时间的前一个节点 auto prev = std::prev(next);
// 根据两个索引，获取两个节点位姿 // Add parameter blocks for the landmark ID if they were not added before std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id); std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);
// Set landmark constant if it is frozen. 不参与优化 // 这里容易记不清，frozen 定义在 struct LandmarkNode if (landmark_node.second.frozen) { problem->SetParameterBlockConstant( C_landmarks->at(landmark_id).translation());
In order to make Cartographer calculations deterministic we strictly time order sensor data using the collator. But you only know in what order to publish data when you have already seen data from all other sources. Sparse sources pose a problem to that strategy.
如果观测是空的，应当发布空的 landmark messages to unblock the queue. 对于fixed frame poses (GPS)也适用。
if (sensor_id.type == SensorId::SensorType::LANDMARK && !collate_landmarks_) { continue; }
如果提供landmark的观测不低于10 Hz，那么可以设置TRAJECTORY_BUILDER.collate_landmarks = on. Cartographer将deterministically运行(对于给定的bag, 使用offline node每次获得的结果是一样的). 如果collate_landmarks = off， landmark observations将跳过sensor queue (so they do not block it if they are sparse) and are injected directly into the pose graph, which is not deterministic.
要求是：标定板在激光正前方120°范围内，并且激光前方2m范围内只存在一个连续的直线线段，所以请在空旷的地方采集数据，不然激光数据会提取错误。需要充分旋转 roll 和 pitch。更直白一点，假设在长方形的标定板中心画了个十字线，那请绕着十字线的两个轴充分旋转，比如绕着竖轴旋转，然后还要绕着横轴旋转。在运行offline程序时，程序会将认为正确的直线会显示为红色。
Load apriltag pose size: 582 terminate called after throwing an instance of 'std::out_of_range' what(): vector::_M_range_check: __n (which is 18446744073709551615) >= this->size() (which is 57) [lasercamcal_ros-1] process has died
有时连录几次bag，都出现这种情况，估计是某个地方在for循环里push_back导致
结果不可观
计算初值阶段
1 2 3 4 5 6 7 8 9
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Notice Notice Notice: system unobservable !!!!!!! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Converts an Eigen Quaternion into a tf Matrix3x3. voidtf::matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t) // Converts a tf Matrix3x3 into an Eigen Quaternion. voidtf::matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e) // Converts an Eigen Affine3d into a tf Transform. voidtf::poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t) // Converts a tf Pose into an Eigen Affine3d. voidtf::poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e) // Converts an Eigen Quaternion into a tf Quaternion. voidtf::quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t) // Converts a tf Quaternion into an Eigen Quaternion. voidtf::quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e) // Converts an Eigen Affine3d into a tf Transform. voidtf::transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t) // Converts a tf Transform into an Eigen Affine3d. voidtf::transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e) // Converts an Eigen Vector3d into a tf Vector3. voidtf::vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t) // Converts a tf Vector3 into an Eigen Vector3d. voidtf::vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e)
/usr/src/gtest/src/gtest.cc:1897:10: error: type ‘const class testing::internal::scoped_ptr<testing::internal::GTestFlagSaver>’ argument given to ‘delete’, expected pointer delete gtest_flag_saver_;
