// 第一个子图或冻结，不优化子图位姿。 也就是不优化初值 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());
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_;
