The first node in the graph SLAM should be fixed. The famous "a tutorial on graph based slam" paper is showing that we can fix a node by adding identity matrix. Why adding identity to the Hessian of the specific node results in fixing that node? What is the theory behind it? Any good material to read?
1 Answer
Not a definitive answer but here are my thoughts: Fixing node $k$ is equivalent to enforcing $\Delta x_{k} = 0$. Now we must show that adding $I$ to diagonal block $H_{kk}$ will result in $\Delta x_{k} = 0$. Let's name $H_{orig}$ the original matrix $H$ before adding $I$ to diagonal block $H_{kk}$, we have: $$ (H_{orig}+\begin{bmatrix} 0 & & & & & \\ & ... & & & & \\ & & 0 & & & \\ & & & I & & \\ & & & & 0 & \\ & & & & & ... \\ & & & & & & 0 \end{bmatrix})\Delta x = -b \Leftrightarrow H_{orig}\Delta x + \Delta x_{k} = -b $$ Can we show that $H_{orig}\Delta x + \Delta x_{k} = -b \Rightarrow \Delta x_{k} = 0$ ?